#include "openManipulatorTrajectoryPlanning.h"
#include "openManipulatorTrajectoryPlanning_types.h"
#include "rtwtypes.h"
#include <string.h>
#include "mwmathutil.h"
#include <emmintrin.h>
#include <stddef.h>
#include "openManipulatorTrajectoryPlanning_private.h"
#include "coder_posix_time.h"
#include <stdlib.h>
#include "rt_logging_mmi.h"
#include "openManipulatorTrajectoryPlanning_capi.h"
#include "openManipulatorTrajectoryPlanning_dt.h"
extern void * CreateDiagnosticAsVoidPtr_wrapper ( const char * id , int nargs
, ... ) ; RTWExtModeInfo * gblRTWExtModeInfo = NULL ; void
raccelForceExtModeShutdown ( boolean_T extModeStartPktReceived ) { if ( !
extModeStartPktReceived ) { boolean_T stopRequested = false ;
rtExtModeWaitForStartPkt ( gblRTWExtModeInfo , 3 , & stopRequested ) ; }
rtExtModeShutdown ( 3 ) ; }
#include "slsv_diagnostic_codegen_c_api.h"
#include "slsa_sim_engine.h"
const int_T gblNumToFiles = 0 ; const int_T gblNumFrFiles = 0 ; const int_T
gblNumFrWksBlocks = 1 ;
#ifdef RSIM_WITH_SOLVER_MULTITASKING
boolean_T gbl_raccel_isMultitasking = 1 ;
#else
boolean_T gbl_raccel_isMultitasking = 0 ;
#endif
boolean_T gbl_raccel_tid01eq = 0 ; int_T gbl_raccel_NumST = 4 ; const char_T
* gbl_raccel_Version = "9.8 (R2022b) 13-May-2022" ; void
raccel_setup_MMIStateLog ( SimStruct * S ) {
#ifdef UseMMIDataLogging
rt_FillStateSigInfoFromMMI ( ssGetRTWLogInfo ( S ) , & ssGetErrorStatus ( S )
) ;
#else
UNUSED_PARAMETER ( S ) ;
#endif
} static DataMapInfo rt_dataMapInfo ; DataMapInfo * rt_dataMapInfoPtr = &
rt_dataMapInfo ; rtwCAPI_ModelMappingInfo * rt_modelMapInfoPtr = & (
rt_dataMapInfo . mmi ) ; const int_T gblNumRootInportBlks = 0 ; const int_T
gblNumModelInputs = 0 ; extern const char * gblInportFileName ; extern
rtInportTUtable * gblInportTUtables ; const int_T gblInportDataTypeIdx [ ] =
{ - 1 } ; const int_T gblInportDims [ ] = { - 1 } ; const int_T
gblInportComplex [ ] = { - 1 } ; const int_T gblInportInterpoFlag [ ] = { - 1
} ; const int_T gblInportContinuous [ ] = { - 1 } ; int_T enableFcnCallFlag [
] = { 1 , 1 , 1 , 1 } ; const char * raccelLoadInputsAndAperiodicHitTimes (
SimStruct * S , const char * inportFileName , int * matFileFormat ) { return
rt_RAccelReadInportsMatFile ( S , inportFileName , matFileFormat ) ; }
#include "simstruc.h"
#include "fixedpoint.h"
#include "slsa_sim_engine.h"
#include "simtarget/slSimTgtSLExecSimBridge.h"
#define nf31gr4fkh (0.005)
B rtB ; X rtX ; DW rtDW ; MassMatrix rtMassMatrix ; static SimStruct model_S
; SimStruct * const rtS = & model_S ; static void m2n1niwoq4 ( e1n154mvbh * *
pEmxArray , int32_T numDimensions ) ; static void iz5okq0a3xi ( alyuhn31fbfz
* pStruct ) ; static void niy4nitoig ( anhhcamq4s3 * * pEmxArray , int32_T
numDimensions ) ; static void iz5okq0a3xik ( ogx4llc4arl * pStruct ) ; static
void ezyujupqop ( ogx4llc4arl pMatrix [ 19 ] ) ; static void calykgtvmm (
klgljlxbmf * * pEmxArray , int32_T numDimensions ) ; static void k4ajowv0ov (
ot4uovzkt0w * pStruct ) ; static void hb45xzxuhu ( ot4uovzkt0w pMatrix [ 19 ]
) ; static void ezyujupqopl ( alyuhn31fbfz pMatrix [ 18 ] ) ; static void
iz5okq0a3x ( mjv2tzlyyigm * pStruct ) ; static void iz5okq0a3xik4 (
lnin0tndd2 * pStruct ) ; static void ot00zilyrs ( ot4uovzkt0w pMatrix [ 18 ]
) ; static void ezyujupqople ( alyuhn31fbfz pMatrix [ 9 ] ) ; static void
ezyujupqoplef ( ogx4llc4arl pMatrix [ 10 ] ) ; static void pjwfe3b5lh (
ot4uovzkt0w pMatrix [ 10 ] ) ; static void iz5okq0a3xik4s ( m0caqfktx0 *
pStruct ) ; static void puw4c3b2a3 ( ecfe5we0wc * pStruct ) ; static void
djhf4h0r0e ( carpjyrt23 * pStruct ) ; static void ecqu5ikgmi ( ecbwrsiemx *
pStruct ) ; static void e3n3mnwk4b ( e1n154mvbh * emxArray , int32_T oldNumel
) ; static void fagcrr3avn ( e1n154mvbh * * pEmxArray ) ; static void
miyl1rq4uj ( klgljlxbmf * emxArray , int32_T oldNumel ) ; static void
aupymb1nfg5 ( anhhcamq4s3 * emxArray , int32_T oldNumel ) ; static void
oamrpiq5ob ( anhhcamq4s3 * * pEmxArray ) ; static ogx4llc4arl * p1kacjwirim (
ogx4llc4arl * obj , real_T maxElements ) ; static alyuhn31fbfz * lwkmqybjwb (
alyuhn31fbfz * obj , ogx4llc4arl * iobj_0 , ot4uovzkt0w * iobj_1 ) ; static
alyuhn31fbfz * e0kx4oauic ( alyuhn31fbfz * obj , ogx4llc4arl * iobj_0 ,
ot4uovzkt0w * iobj_1 ) ; static alyuhn31fbfz * mpzyhljwzq ( alyuhn31fbfz *
obj , ogx4llc4arl * iobj_0 , ot4uovzkt0w * iobj_1 ) ; static alyuhn31fbfz *
axww4prr0o ( alyuhn31fbfz * obj , ogx4llc4arl * iobj_0 , ot4uovzkt0w * iobj_1
) ; static alyuhn31fbfz * oh2xpcdbml ( alyuhn31fbfz * obj , ogx4llc4arl *
iobj_0 , ot4uovzkt0w * iobj_1 ) ; static alyuhn31fbfz * hcmkep3fjc (
alyuhn31fbfz * obj , ogx4llc4arl * iobj_0 , ot4uovzkt0w * iobj_1 ) ; static
alyuhn31fbfz * eearo4zkrq ( alyuhn31fbfz * obj , ogx4llc4arl * iobj_0 ,
ot4uovzkt0w * iobj_1 ) ; static alyuhn31fbfz * et05vlhztn ( alyuhn31fbfz *
obj , ogx4llc4arl * iobj_0 , ot4uovzkt0w * iobj_1 ) ; static alyuhn31fbfz *
oct3agbiik ( alyuhn31fbfz * obj , ogx4llc4arl * iobj_0 , ot4uovzkt0w * iobj_1
) ; static alyuhn31fbfz * jphupobpz2 ( alyuhn31fbfz * obj , ogx4llc4arl *
iobj_0 , ot4uovzkt0w * iobj_1 ) ; static alyuhn31fbfz * bl5mnhfgqx (
alyuhn31fbfz * obj , ogx4llc4arl * iobj_0 , ot4uovzkt0w * iobj_1 ) ; static
alyuhn31fbfz * gwskexfjch ( alyuhn31fbfz * obj , ogx4llc4arl * iobj_0 ,
ot4uovzkt0w * iobj_1 ) ; static alyuhn31fbfz * nuiglfj4n2 ( alyuhn31fbfz *
obj , ogx4llc4arl * iobj_0 , ot4uovzkt0w * iobj_1 ) ; static alyuhn31fbfz *
nxwlc1tqzg ( alyuhn31fbfz * obj , ogx4llc4arl * iobj_0 , ot4uovzkt0w * iobj_1
) ; static alyuhn31fbfz * gtyzt122qi ( alyuhn31fbfz * obj , ogx4llc4arl *
iobj_0 , ot4uovzkt0w * iobj_1 ) ; static alyuhn31fbfz * dyhbwd1qgw (
alyuhn31fbfz * obj , ogx4llc4arl * iobj_0 , ot4uovzkt0w * iobj_1 ) ; static
ot4uovzkt0w * g1japxta3w ( ot4uovzkt0w * obj , const e1n154mvbh * jname ) ;
static mjv2tzlyyigm * jgbwvk33lx ( mjv2tzlyyigm * obj ) ; static void
csqey0xxgza ( uint32_T mt [ 625 ] , uint32_T u [ 2 ] ) ; static boolean_T
enplvw3xkz ( const uint32_T mt [ 625 ] ) ; static void jlodq4k5z5 ( real_T r
[ 5 ] ) ; static boolean_T e3fq41hhh0 ( const e1n154mvbh * a , const
e1n154mvbh * b ) ; static real_T jjyjgxfdua ( m0caqfktx0 * obj , const
e1n154mvbh * bodyname ) ; static void bdhoypqe5x ( klgljlxbmf * * pEmxArray )
; static alyuhn31fbfz * pfo1apkwc0 ( alyuhn31fbfz * obj , ogx4llc4arl *
iobj_0 , ot4uovzkt0w * iobj_1 , alyuhn31fbfz * iobj_2 ) ; static void
jem4veejvh ( m0caqfktx0 * obj , alyuhn31fbfz * bodyin , const e1n154mvbh *
parentName , ogx4llc4arl * iobj_0 , ot4uovzkt0w * iobj_1 , alyuhn31fbfz *
iobj_2 ) ; static void omjnvaudda ( carpjyrt23 * obj , mjv2tzlyyigm *
rigidbodytree , ot4uovzkt0w * iobj_0 , alyuhn31fbfz * iobj_1 , ogx4llc4arl *
iobj_2 , m0caqfktx0 * iobj_3 ) ; static void ep2455cw5ms ( ecbwrsiemx * obj )
; static void m4dtuohf4b ( ot4uovzkt0 * pStruct ) ; static void niy4nitoigo (
anhhcamq4s * * pEmxArray , int32_T numDimensions ) ; static void iwumdx5q3w (
ogx4llc4ar * pStruct ) ; static void nwwh5wd3rb ( alyuhn31fb * pStruct ) ;
static void ezyujupqoplefp ( alyuhn31fb pMatrix [ 18 ] ) ; static void
iz5okq0a3xik4sr ( mjv2tzlyyi * pStruct ) ; static void ecqu5ikgmie (
evzhsot2uw * pStruct ) ; static void aupymb1nfg ( anhhcamq4s * emxArray ,
int32_T oldNumel ) ; static void oamrpiq5obm ( anhhcamq4s * * pEmxArray ) ;
static ogx4llc4ar * p1kacjwiri ( ogx4llc4ar * obj ) ; static alyuhn31fb *
nnpwrpxrzk ( alyuhn31fb * obj ) ; static alyuhn31fb * nnpwrpxrzkq (
alyuhn31fb * obj ) ; static alyuhn31fb * nnpwrpxrzkqi ( alyuhn31fb * obj ) ;
static alyuhn31fb * nnpwrpxrzkqix ( alyuhn31fb * obj ) ; static alyuhn31fb *
nnpwrpxrzkqixz ( alyuhn31fb * obj ) ; static void kff4ccpwjv ( mjv2tzlyyi *
obj , alyuhn31fb * iobj_0 ) ; static alyuhn31fb * nnpwrpxrzkqixzq (
alyuhn31fb * obj ) ; static alyuhn31fb * b4tj0yrbq0 ( alyuhn31fb * obj ) ;
static alyuhn31fb * j00sarju5h ( alyuhn31fb * obj ) ; static alyuhn31fb *
bgbooan2dw ( alyuhn31fb * obj ) ; static alyuhn31fb * g22snpgfll ( alyuhn31fb
* obj ) ; static alyuhn31fb * o0s2nkqu2g ( alyuhn31fb * obj ) ; static void
ep2455cw5m ( evzhsot2uw * obj ) ; static void fx50peix55 ( ot4uovzkt0wp *
pStruct ) ; static void b4adihsbvg ( alyuhn31fbf * pStruct ) ; static void
ezyujupqoplefpe ( alyuhn31fbf pMatrix [ 18 ] ) ; static void hzaeefljc4 (
mjv2tzlyyig * pStruct ) ; static void ecqu5ikgmiel ( jd4q4eqa1g * pStruct ) ;
static alyuhn31fbf * f502siufoq ( alyuhn31fbf * obj ) ; static alyuhn31fbf *
au3yg4mglc ( alyuhn31fbf * obj ) ; static alyuhn31fbf * ir0wvh3inp (
alyuhn31fbf * obj ) ; static alyuhn31fbf * neqqfka14z ( alyuhn31fbf * obj ) ;
static alyuhn31fbf * lafe2rmqud ( alyuhn31fbf * obj ) ; static void
kff4ccpwjvj ( mjv2tzlyyig * obj , alyuhn31fbf * iobj_0 ) ; static alyuhn31fbf
* f5m4gscdrl ( alyuhn31fbf * obj ) ; static alyuhn31fbf * fpyihj4bu5 (
alyuhn31fbf * obj ) ; static alyuhn31fbf * dgvymmhbap ( alyuhn31fbf * obj ) ;
static alyuhn31fbf * dqkobvx1cl ( alyuhn31fbf * obj ) ; static alyuhn31fbf *
ax1vg3c3wq ( alyuhn31fbf * obj ) ; static alyuhn31fbf * lful00nwr0 (
alyuhn31fbf * obj ) ; static alyuhn31fbf * ph5qebjzgw ( alyuhn31fbf * obj ) ;
static void ep2455cw5msd ( jd4q4eqa1g * obj ) ; static void fv2pu0tbva (
lbiyaleu3n * * pEmxArray , int32_T numDimensions ) ; static void n10lmcng5l (
lbiyaleu3n * emxArray , int32_T oldNumel ) ; static void bqvhlmvfpz (
lbiyaleu3n * * pEmxArray ) ; static void lij5c2bvo0 ( m0caqfktx0 * obj ,
klgljlxbmf * limits ) ; static void lh3fakbjip ( boolean_T in1 [ 6 ] , const
real_T in2 [ 6 ] , const klgljlxbmf * in3 ) ; static void offa2br1khoegki (
boolean_T in1 [ 6 ] , const real_T in2 [ 6 ] , const klgljlxbmf * in3 ) ;
static void mfmaegtoxt ( const boolean_T x [ 6 ] , int32_T i_data [ ] ,
int32_T * i_size ) ; static void ot2wucsrdj ( real_T * tstart_tv_sec , real_T
* tstart_tv_nsec ) ; static void e3pxqiajks ( m0caqfktx0 * obj , alyuhn31fbfz
* body , klgljlxbmf * indices ) ; static void k4jhhplgmj ( m0caqfktx0 * obj ,
alyuhn31fbfz * body1 , alyuhn31fbfz * body2 , klgljlxbmf * indices ) ; static
void cnixr4pd3ac ( const ot4uovzkt0w * obj , real_T ax [ 3 ] ) ; static void
lpd2uo4ns5 ( real_T varargin_1 , real_T varargin_2 , real_T varargin_3 ,
real_T varargin_4 , real_T varargin_5 , real_T varargin_6 , real_T varargin_7
, real_T varargin_8 , real_T varargin_9 , real_T y [ 9 ] ) ; static void
l5ks21uytp ( const real_T A [ 36 ] , const klgljlxbmf * B_p , klgljlxbmf * C
) ; static void p3z13glynv ( m0caqfktx0 * obj , const real_T qv [ 6 ] , const
e1n154mvbh * body1Name , real_T T_data [ ] , int32_T T_size [ 2 ] ,
klgljlxbmf * Jac ) ; static creal_T hv02b2lq4w ( const creal_T x ) ; static
real_T iq4v5bez02 ( int32_T n , const real_T x [ 9 ] , int32_T ix0 ) ; static
real_T hxwoj44b1k ( int32_T n , const real_T x [ 9 ] , int32_T ix0 , const
real_T y [ 9 ] , int32_T iy0 ) ; static void dqzyu4u4dr ( int32_T n , real_T
a , int32_T ix0 , const real_T y [ 9 ] , int32_T iy0 , real_T b_y [ 9 ] ) ;
static real_T iq4v5bez02k ( const real_T x [ 3 ] , int32_T ix0 ) ; static
void dqzyu4u4dryag ( int32_T n , real_T a , const real_T x [ 9 ] , int32_T
ix0 , real_T y [ 3 ] , int32_T iy0 ) ; static void dqzyu4u4drya ( int32_T n ,
real_T a , const real_T x [ 3 ] , int32_T ix0 , const real_T y [ 9 ] ,
int32_T iy0 , real_T b_y [ 9 ] ) ; static void h1cxhqfs1e ( const real_T x [
9 ] , int32_T ix0 , int32_T iy0 , real_T b_x [ 9 ] ) ; static void pwomuwmg0o
( real_T a , real_T b , real_T * b_a , real_T * b_b , real_T * c , real_T * s
) ; static void jqns5sfonc ( const real_T x [ 9 ] , int32_T ix0 , int32_T iy0
, real_T c , real_T s , real_T b_x [ 9 ] ) ; static void o5vuefzdbw ( const
real_T A [ 9 ] , real_T U [ 9 ] , real_T s [ 3 ] , real_T V [ 9 ] ) ; static
void pbvdpfzges ( const real_T x [ 6 ] , lnin0tndd2 * args , real_T * cost ,
real_T W [ 36 ] , klgljlxbmf * Jac , lnin0tndd2 * * b_args ) ; static void
l45e0kru51 ( m5kgub1d5k * * pEmxArray , int32_T numDimensions ) ; static void
ks3ealo3sd ( ncvluzgkod * * pEmxArray , int32_T numDimensions ) ; static void
dw1xs30knw ( m5kgub1d5k * emxArray , int32_T oldNumel ) ; static void
l5ks21uytpa ( const klgljlxbmf * A , const real_T B_e [ 6 ] , klgljlxbmf * C
) ; static void offa2br1khoegk ( m5kgub1d5k * in1 , const klgljlxbmf * in2 ,
const ecfe5we0wc * in3 ) ; static void os5bv5kg0d ( ncvluzgkod * emxArray ,
int32_T oldNumel ) ; static void l5ks21uytpaq ( const real_T A [ 36 ] , const
klgljlxbmf * B_i , klgljlxbmf * C ) ; static real_T fdzsuttcyn1 ( const
real_T x [ 6 ] ) ; static real_T ethycgs32k ( real_T tstart_tv_sec , real_T
tstart_tv_nsec ) ; static void l5ks21uytpaq2 ( const klgljlxbmf * A , const
klgljlxbmf * B_m , klgljlxbmf * C ) ; static real_T iq4v5bez02ka ( int32_T n
, const klgljlxbmf * x , int32_T ix0 ) ; static void preyilpnl4 ( ncvluzgkod
* * pEmxArray ) ; static void ldurgmcftq ( const klgljlxbmf * A , klgljlxbmf
* b_A , klgljlxbmf * tau , ncvluzgkod * jpvt ) ; static void dh3xcvj5bl (
int32_T m , int32_T n , const klgljlxbmf * A , int32_T lda , klgljlxbmf * b_A
, ncvluzgkod * ipiv , int32_T * info ) ; static void hjklbkk1g0 ( int32_T m ,
int32_T n , const klgljlxbmf * A , int32_T lda , const klgljlxbmf * B_g ,
int32_T ldb , klgljlxbmf * b_B ) ; static void cp5ozjiqhu ( const klgljlxbmf
* A , const klgljlxbmf * B_i , klgljlxbmf * Y ) ; static void naapodi4lu (
m5kgub1d5k * * pEmxArray ) ; static boolean_T aofopy35eo ( const ecfe5we0wc *
obj , const real_T Hg [ 6 ] , const klgljlxbmf * alpha ) ; static void
bqduk5sgwd ( const klgljlxbmf * x , klgljlxbmf * y ) ; static void gka31aeazw
( const klgljlxbmf * v , klgljlxbmf * d ) ; static void dmv3200pfy ( const
klgljlxbmf * x , real_T * ex , int32_T * idx ) ; static void offa2br1khoeg (
const klgljlxbmf * in1 , const klgljlxbmf * in2 , real_T * out1 , int32_T *
out2 ) ; static boolean_T pahyhnfmn2 ( const m5kgub1d5k * x ) ; static void
offa2br1khoe ( klgljlxbmf * in1 , const klgljlxbmf * in2 , const klgljlxbmf *
in3 ) ; static void mfmaegtoxtz ( const m5kgub1d5k * x , ncvluzgkod * i ) ;
static void heltobv2ai ( const klgljlxbmf * x , real_T * ex , int32_T * idx )
; static void bsiqjn4ttti1 ( klgljlxbmf * in1 , const klgljlxbmf * in2 ) ;
static void offa2br1kho ( real_T in1 [ 36 ] , const real_T in2 [ 36 ] , const
real_T in3 [ 6 ] , const klgljlxbmf * in4 , real_T in5 ) ; static boolean_T
np5inobdqs ( const real_T B_f [ 36 ] ) ; static void l5ks21uytpaq2zf ( const
klgljlxbmf * A , const real_T B_j [ 36 ] , klgljlxbmf * C ) ; static void
l5ks21uytpaq2z ( const klgljlxbmf * A , const klgljlxbmf * B_p , klgljlxbmf *
C ) ; static void bsiqjn4ttti ( real_T in1 [ 36 ] , const klgljlxbmf * in2 )
; static void ngq4y5c0bx ( ecfe5we0wc * obj , real_T xSol [ 6 ] , gkb0igonbu
* exitFlag , real_T * err , real_T * iter ) ; static void inyjui0sxx ( const
real_T varargin_1 [ 2 ] , klgljlxbmf * r ) ; static void bsiqjn4ttt (
klgljlxbmf * in1 , const klgljlxbmf * in2 ) ; static void cu5fmhs0ay (
klgljlxbmf * in1 , const klgljlxbmf * in2 ) ; static void jlodq4k5z5m (
real_T varargin_1 , klgljlxbmf * r ) ; static void offa2br1kh ( klgljlxbmf *
in1 , const klgljlxbmf * in2 , const klgljlxbmf * in3 ) ; static void
d2iciofqsp ( ecfe5we0wc * obj , const real_T seed [ 6 ] , real_T xSol [ 6 ] ,
real_T * solutionInfo_Iterations , real_T * solutionInfo_RRAttempts , real_T
* solutionInfo_Error , real_T * solutionInfo_ExitFlag , char_T
solutionInfo_Status_data [ ] , int32_T solutionInfo_Status_size [ 2 ] ) ;
static void bev1y1zpzb ( pr5ntxwpwx * * pEmxArray , int32_T numDimensions ) ;
static void amnzbgdo2d ( pr5ntxwpwx * emxArray , int32_T oldNumel ) ; static
void migotnoom2 ( pr5ntxwpwx * * pEmxArray ) ; static void c4f0e12yvkr (
carpjyrt23 * obj , real_T initialGuess [ 6 ] , real_T *
solutionInfo_Iterations , real_T * solutionInfo_NumRandomRestarts , real_T *
solutionInfo_PoseErrorNorm , real_T * solutionInfo_ExitFlag , char_T
solutionInfo_Status_data [ ] , int32_T solutionInfo_Status_size [ 2 ] ) ;
static void gai2oub5e3 ( carpjyrt23 * obj , const real_T tform [ 16 ] , const
real_T weights [ 6 ] , const real_T initialGuess [ 6 ] , real_T QSol [ 6 ] ,
real_T * solutionInfo_Iterations , real_T * solutionInfo_PoseErrorNorm ,
real_T * solutionInfo_ExitFlag , char_T solutionInfo_Status_data [ ] ,
int32_T solutionInfo_Status_size [ 2 ] ) ; static void jarne05rzi (
hzpfvcreu1 * * pEmxArray , int32_T numDimensions ) ; static void f33velrxf0 (
hzpfvcreu1 * emxArray , int32_T oldNumel ) ; static void cnixr4pd3a ( const
ot4uovzkt0 * obj , real_T ax [ 3 ] ) ; static void jhl11noygj ( hzpfvcreu1 *
* pEmxArray ) ; static void cdtxuhnnn2 ( hzpfvcreu1o * * pEmxArray , int32_T
numDimensions ) ; static void obcr1nckmu ( hzpfvcreu1o * emxArray , int32_T
oldNumel ) ; static void cnixr4pd3aci ( const ot4uovzkt0wp * obj , real_T ax
[ 3 ] ) ; static void n4riw4ioyca ( const ot4uovzkt0wp * obj , const real_T
q_data [ ] , const int32_T * q_size , real_T T [ 16 ] ) ; static void
n4riw4ioyc ( const ot4uovzkt0wp * obj , real_T T [ 16 ] ) ; static void
oxlwy3eg3q ( hzpfvcreu1o * * pEmxArray ) ; static void j0alujw22n (
alyuhn31fbfz * pStruct ) ; static void j0alujw22n13 ( ogx4llc4arl * pStruct )
; static void ovdvrmzlss ( ogx4llc4arl pMatrix [ 19 ] ) ; static void
jfnfdyiidb ( ot4uovzkt0w * pStruct ) ; static void dqwgmpsyro ( ot4uovzkt0w
pMatrix [ 19 ] ) ; static void ovdvrmzlss0 ( alyuhn31fbfz pMatrix [ 18 ] ) ;
static void j0alujw22n1 ( mjv2tzlyyigm * pStruct ) ; static void
j0alujw22n13v ( lnin0tndd2 * pStruct ) ; static void ojga4a1v1p ( ot4uovzkt0w
pMatrix [ 18 ] ) ; static void ovdvrmzlss0z ( alyuhn31fbfz pMatrix [ 9 ] ) ;
static void ovdvrmzlss0z4 ( ogx4llc4arl pMatrix [ 10 ] ) ; static void
em2zaf1fxr ( ot4uovzkt0w pMatrix [ 10 ] ) ; static void j0alujw22n13v5 (
m0caqfktx0 * pStruct ) ; static void n5jki5o22u ( ecfe5we0wc * pStruct ) ;
static void b15a2b3iyt ( carpjyrt23 * pStruct ) ; static void omslekmpnx (
ecbwrsiemx * pStruct ) ; static void eq1frudh3p ( ot4uovzkt0 * pStruct ) ;
static void fhh2u1a23n ( ogx4llc4ar * pStruct ) ; static void j0alujw22n13v5u
( alyuhn31fb * pStruct ) ; static void ovdvrmzlss0z4t ( alyuhn31fb pMatrix [
18 ] ) ; static void bfznjjiplu ( mjv2tzlyyi * pStruct ) ; static void
omslekmpnxk ( evzhsot2uw * pStruct ) ; static void k1aewmoyxc ( ot4uovzkt0wp
* pStruct ) ; static void ergoqrpdey ( alyuhn31fbf * pStruct ) ; static void
ovdvrmzlss0z4tv ( alyuhn31fbf pMatrix [ 18 ] ) ; static void e3vs32ulkf (
mjv2tzlyyig * pStruct ) ; static void omslekmpnxkp ( jd4q4eqa1g * pStruct ) ;
int32_T div_s32 ( int32_T numerator , int32_T denominator ) { int32_T
quotient ; uint32_T tempAbsQuotient ; if ( denominator == 0 ) { quotient =
numerator >= 0 ? MAX_int32_T : MIN_int32_T ; } else { tempAbsQuotient = (
numerator < 0 ? ~ ( uint32_T ) numerator + 1U : ( uint32_T ) numerator ) / (
denominator < 0 ? ~ ( uint32_T ) denominator + 1U : ( uint32_T ) denominator
) ; quotient = ( numerator < 0 ) != ( denominator < 0 ) ? - ( int32_T )
tempAbsQuotient : ( int32_T ) tempAbsQuotient ; } return quotient ; } int32_T
div_nde_s32_floor ( int32_T numerator , int32_T denominator ) { return ( ( (
numerator < 0 ) != ( denominator < 0 ) ) && ( numerator % denominator != 0 )
? - 1 : 0 ) + numerator / denominator ; } static void m2n1niwoq4 ( e1n154mvbh
* * pEmxArray , int32_T numDimensions ) { e1n154mvbh * emxArray ; int32_T i ;
* pEmxArray = ( e1n154mvbh * ) malloc ( sizeof ( e1n154mvbh ) ) ; emxArray =
* pEmxArray ; emxArray -> data = ( char_T * ) NULL ; emxArray ->
numDimensions = numDimensions ; emxArray -> size = ( int32_T * ) malloc (
sizeof ( int32_T ) * ( uint32_T ) numDimensions ) ; emxArray -> allocatedSize
= 0 ; emxArray -> canFreeData = true ; for ( i = 0 ; i < numDimensions ; i ++
) { emxArray -> size [ i ] = 0 ; } } static void iz5okq0a3xi ( alyuhn31fbfz *
pStruct ) { m2n1niwoq4 ( & pStruct -> NameInternal , 2 ) ; } static void
niy4nitoig ( anhhcamq4s3 * * pEmxArray , int32_T numDimensions ) {
anhhcamq4s3 * emxArray ; int32_T i ; * pEmxArray = ( anhhcamq4s3 * ) malloc (
sizeof ( anhhcamq4s3 ) ) ; emxArray = * pEmxArray ; emxArray -> data = (
om2wdj4ounp * ) NULL ; emxArray -> numDimensions = numDimensions ; emxArray
-> size = ( int32_T * ) malloc ( sizeof ( int32_T ) * ( uint32_T )
numDimensions ) ; emxArray -> allocatedSize = 0 ; emxArray -> canFreeData =
true ; for ( i = 0 ; i < numDimensions ; i ++ ) { emxArray -> size [ i ] = 0
; } } static void iz5okq0a3xik ( ogx4llc4arl * pStruct ) { niy4nitoig ( &
pStruct -> CollisionGeometries , 2 ) ; } static void ezyujupqop ( ogx4llc4arl
pMatrix [ 19 ] ) { int32_T i ; for ( i = 0 ; i < 19 ; i ++ ) { iz5okq0a3xik (
& pMatrix [ i ] ) ; } } static void calykgtvmm ( klgljlxbmf * * pEmxArray ,
int32_T numDimensions ) { klgljlxbmf * emxArray ; int32_T i ; * pEmxArray = (
klgljlxbmf * ) malloc ( sizeof ( klgljlxbmf ) ) ; emxArray = * pEmxArray ;
emxArray -> data = ( real_T * ) NULL ; emxArray -> numDimensions =
numDimensions ; emxArray -> size = ( int32_T * ) malloc ( sizeof ( int32_T )
* ( uint32_T ) numDimensions ) ; emxArray -> allocatedSize = 0 ; emxArray ->
canFreeData = true ; for ( i = 0 ; i < numDimensions ; i ++ ) { emxArray ->
size [ i ] = 0 ; } } static void k4ajowv0ov ( ot4uovzkt0w * pStruct ) {
m2n1niwoq4 ( & pStruct -> Type , 2 ) ; calykgtvmm ( & pStruct ->
MotionSubspace , 2 ) ; m2n1niwoq4 ( & pStruct -> NameInternal , 2 ) ;
calykgtvmm ( & pStruct -> PositionLimitsInternal , 2 ) ; calykgtvmm ( &
pStruct -> HomePositionInternal , 1 ) ; } static void hb45xzxuhu (
ot4uovzkt0w pMatrix [ 19 ] ) { int32_T i ; for ( i = 0 ; i < 19 ; i ++ ) {
k4ajowv0ov ( & pMatrix [ i ] ) ; } } static void ezyujupqopl ( alyuhn31fbfz
pMatrix [ 18 ] ) { int32_T i ; for ( i = 0 ; i < 18 ; i ++ ) { iz5okq0a3xi (
& pMatrix [ i ] ) ; } } static void iz5okq0a3x ( mjv2tzlyyigm * pStruct ) {
iz5okq0a3xi ( & pStruct -> Base ) ; ezyujupqop ( pStruct -> _pobj0 ) ;
hb45xzxuhu ( pStruct -> _pobj1 ) ; ezyujupqopl ( pStruct -> _pobj2 ) ; }
static void iz5okq0a3xik4 ( lnin0tndd2 * pStruct ) { m2n1niwoq4 ( & pStruct
-> BodyName , 2 ) ; calykgtvmm ( & pStruct -> ErrTemp , 1 ) ; calykgtvmm ( &
pStruct -> GradTemp , 1 ) ; } static void ot00zilyrs ( ot4uovzkt0w pMatrix [
18 ] ) { int32_T i ; for ( i = 0 ; i < 18 ; i ++ ) { k4ajowv0ov ( & pMatrix [
i ] ) ; } } static void ezyujupqople ( alyuhn31fbfz pMatrix [ 9 ] ) { int32_T
i ; for ( i = 0 ; i < 9 ; i ++ ) { iz5okq0a3xi ( & pMatrix [ i ] ) ; } }
static void ezyujupqoplef ( ogx4llc4arl pMatrix [ 10 ] ) { int32_T i ; for (
i = 0 ; i < 10 ; i ++ ) { iz5okq0a3xik ( & pMatrix [ i ] ) ; } } static void
pjwfe3b5lh ( ot4uovzkt0w pMatrix [ 10 ] ) { int32_T i ; for ( i = 0 ; i < 10
; i ++ ) { k4ajowv0ov ( & pMatrix [ i ] ) ; } } static void iz5okq0a3xik4s (
m0caqfktx0 * pStruct ) { iz5okq0a3xi ( & pStruct -> Base ) ; ezyujupqople (
pStruct -> _pobj0 ) ; ezyujupqoplef ( pStruct -> _pobj1 ) ; pjwfe3b5lh (
pStruct -> _pobj2 ) ; } static void puw4c3b2a3 ( ecfe5we0wc * pStruct ) {
calykgtvmm ( & pStruct -> ConstraintMatrix , 2 ) ; calykgtvmm ( & pStruct ->
ConstraintBound , 1 ) ; } static void djhf4h0r0e ( carpjyrt23 * pStruct ) {
calykgtvmm ( & pStruct -> Limits , 2 ) ; iz5okq0a3xik4 ( & pStruct -> _pobj0
) ; ot00zilyrs ( pStruct -> _pobj1 ) ; ezyujupqople ( pStruct -> _pobj2 ) ;
ezyujupqop ( pStruct -> _pobj3 ) ; iz5okq0a3xik4s ( & pStruct -> _pobj4 ) ;
puw4c3b2a3 ( & pStruct -> _pobj5 ) ; } static void ecqu5ikgmi ( ecbwrsiemx *
pStruct ) { iz5okq0a3x ( & pStruct -> TreeInternal ) ; djhf4h0r0e ( & pStruct
-> IKInternal ) ; } static void e3n3mnwk4b ( e1n154mvbh * emxArray , int32_T
oldNumel ) { int32_T i ; int32_T newNumel ; void * newData ; if ( oldNumel <
0 ) { oldNumel = 0 ; } newNumel = 1 ; for ( i = 0 ; i < emxArray ->
numDimensions ; i ++ ) { newNumel *= emxArray -> size [ i ] ; } if ( newNumel
> emxArray -> allocatedSize ) { i = emxArray -> allocatedSize ; if ( i < 16 )
{ i = 16 ; } while ( i < newNumel ) { if ( i > 1073741823 ) { i = MAX_int32_T
; } else { i <<= 1 ; } } newData = calloc ( ( uint32_T ) i , sizeof ( char_T
) ) ; if ( emxArray -> data != NULL ) { memcpy ( newData , emxArray -> data ,
sizeof ( char_T ) * ( uint32_T ) oldNumel ) ; if ( emxArray -> canFreeData )
{ free ( emxArray -> data ) ; } } emxArray -> data = ( char_T * ) newData ;
emxArray -> allocatedSize = i ; emxArray -> canFreeData = true ; } } static
void fagcrr3avn ( e1n154mvbh * * pEmxArray ) { if ( * pEmxArray != (
e1n154mvbh * ) NULL ) { if ( ( ( * pEmxArray ) -> data != ( char_T * ) NULL )
&& ( * pEmxArray ) -> canFreeData ) { free ( ( * pEmxArray ) -> data ) ; }
free ( ( * pEmxArray ) -> size ) ; free ( * pEmxArray ) ; * pEmxArray = (
e1n154mvbh * ) NULL ; } } static void miyl1rq4uj ( klgljlxbmf * emxArray ,
int32_T oldNumel ) { int32_T i ; int32_T newNumel ; void * newData ; if (
oldNumel < 0 ) { oldNumel = 0 ; } newNumel = 1 ; for ( i = 0 ; i < emxArray
-> numDimensions ; i ++ ) { newNumel *= emxArray -> size [ i ] ; } if (
newNumel > emxArray -> allocatedSize ) { i = emxArray -> allocatedSize ; if (
i < 16 ) { i = 16 ; } while ( i < newNumel ) { if ( i > 1073741823 ) { i =
MAX_int32_T ; } else { i <<= 1 ; } } newData = calloc ( ( uint32_T ) i ,
sizeof ( real_T ) ) ; if ( emxArray -> data != NULL ) { memcpy ( newData ,
emxArray -> data , sizeof ( real_T ) * ( uint32_T ) oldNumel ) ; if (
emxArray -> canFreeData ) { free ( emxArray -> data ) ; } } emxArray -> data
= ( real_T * ) newData ; emxArray -> allocatedSize = i ; emxArray ->
canFreeData = true ; } } static void aupymb1nfg5 ( anhhcamq4s3 * emxArray ,
int32_T oldNumel ) { int32_T i ; int32_T newNumel ; void * newData ; if (
oldNumel < 0 ) { oldNumel = 0 ; } newNumel = 1 ; for ( i = 0 ; i < emxArray
-> numDimensions ; i ++ ) { newNumel *= emxArray -> size [ i ] ; } if (
newNumel > emxArray -> allocatedSize ) { i = emxArray -> allocatedSize ; if (
i < 16 ) { i = 16 ; } while ( i < newNumel ) { if ( i > 1073741823 ) { i =
MAX_int32_T ; } else { i <<= 1 ; } } newData = calloc ( ( uint32_T ) i ,
sizeof ( om2wdj4ounp ) ) ; if ( emxArray -> data != NULL ) { memcpy ( newData
, emxArray -> data , sizeof ( om2wdj4ounp ) * ( uint32_T ) oldNumel ) ; if (
emxArray -> canFreeData ) { free ( emxArray -> data ) ; } } emxArray -> data
= ( om2wdj4ounp * ) newData ; emxArray -> allocatedSize = i ; emxArray ->
canFreeData = true ; } } static void oamrpiq5ob ( anhhcamq4s3 * * pEmxArray )
{ if ( * pEmxArray != ( anhhcamq4s3 * ) NULL ) { if ( ( ( * pEmxArray ) ->
data != ( om2wdj4ounp * ) NULL ) && ( * pEmxArray ) -> canFreeData ) { free (
( * pEmxArray ) -> data ) ; } free ( ( * pEmxArray ) -> size ) ; free ( *
pEmxArray ) ; * pEmxArray = ( anhhcamq4s3 * ) NULL ; } } static ogx4llc4arl *
p1kacjwirim ( ogx4llc4arl * obj , real_T maxElements ) { void *
defaultCollisionObj_GeometryInternal ; anhhcamq4s3 * e ; ogx4llc4arl * b_obj
; real_T c ; int32_T b_i ; int32_T d ; int8_T localPose [ 16 ] ; static const
int8_T tmp [ 16 ] = { 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 ,
0 , 1 } ; int32_T i ; obj -> Size = 0.0 ; b_obj = obj ; obj -> MaxElements =
maxElements ; niy4nitoig ( & e , 2 ) ; i = e -> size [ 0 ] * e -> size [ 1 ]
; e -> size [ 1 ] = ( int32_T ) obj -> MaxElements ; aupymb1nfg5 ( e , i ) ;
i = obj -> CollisionGeometries -> size [ 0 ] * obj -> CollisionGeometries ->
size [ 1 ] ; obj -> CollisionGeometries -> size [ 0 ] = 1 ; obj ->
CollisionGeometries -> size [ 1 ] = e -> size [ 1 ] ; oamrpiq5ob ( & e ) ;
aupymb1nfg5 ( obj -> CollisionGeometries , i ) ;
defaultCollisionObj_GeometryInternal = NULL ; for ( i = 0 ; i < 16 ; i ++ ) {
localPose [ i ] = tmp [ i ] ; } c = obj -> MaxElements ; d = ( int32_T ) c -
1 ; for ( b_i = 0 ; b_i <= d ; b_i ++ ) { obj -> CollisionGeometries -> data
[ b_i ] . CollisionPrimitive = defaultCollisionObj_GeometryInternal ; for ( i
= 0 ; i < 16 ; i ++ ) { obj -> CollisionGeometries -> data [ b_i ] .
LocalPose [ i ] = localPose [ i ] ; } for ( i = 0 ; i < 16 ; i ++ ) { obj ->
CollisionGeometries -> data [ b_i ] . WorldPose [ i ] = localPose [ i ] ; } }
obj -> matlabCodegenIsDeleted = false ; return b_obj ; } static alyuhn31fbfz
* lwkmqybjwb ( alyuhn31fbfz * obj , ogx4llc4arl * iobj_0 , ot4uovzkt0w *
iobj_1 ) { alyuhn31fbfz * b_obj ; e1n154mvbh * switch_expression ; real_T
poslim_data [ 12 ] ; int32_T b_kstr ; int32_T loop_ub ; char_T b_p [ 9 ] ;
char_T b [ 8 ] ; int8_T msubspace_data [ 36 ] ; int8_T b_I [ 9 ] ; int8_T tmp
[ 6 ] ; boolean_T b_bool ; static const char_T tmp_p [ 10 ] = { 'd' , 'u' ,
'm' , 'm' , 'y' , 'b' , 'o' , 'd' , 'y' , '1' } ; static const int8_T tmp_e [
16 ] = { 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 } ;
static const char_T tmp_i [ 14 ] = { 'd' , 'u' , 'm' , 'm' , 'y' , 'b' , 'o'
, 'd' , 'y' , '1' , '_' , 'j' , 'n' , 't' } ; static const char_T tmp_m [ 5 ]
= { 'f' , 'i' , 'x' , 'e' , 'd' } ; static const char_T tmp_g [ 8 ] = { 'r' ,
'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ; static const char_T tmp_j [ 9 ] =
{ 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' , 'i' , 'c' } ; int32_T exitg1 ;
b_obj = obj ; b_kstr = obj -> NameInternal -> size [ 0 ] * obj ->
NameInternal -> size [ 1 ] ; obj -> NameInternal -> size [ 0 ] = 1 ; obj ->
NameInternal -> size [ 1 ] = 10 ; e3n3mnwk4b ( obj -> NameInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 10 ; b_kstr ++ ) { obj -> NameInternal -> data
[ b_kstr ] = tmp_p [ b_kstr ] ; } iobj_1 -> InTree = false ; for ( b_kstr = 0
; b_kstr < 16 ; b_kstr ++ ) { iobj_1 -> JointToParentTransform [ b_kstr ] =
tmp_e [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { iobj_1 ->
ChildToJointTransform [ b_kstr ] = tmp_e [ b_kstr ] ; } b_kstr = iobj_1 ->
NameInternal -> size [ 0 ] * iobj_1 -> NameInternal -> size [ 1 ] ; iobj_1 ->
NameInternal -> size [ 0 ] = 1 ; iobj_1 -> NameInternal -> size [ 1 ] = 14 ;
e3n3mnwk4b ( iobj_1 -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr <
14 ; b_kstr ++ ) { iobj_1 -> NameInternal -> data [ b_kstr ] = tmp_i [ b_kstr
] ; } b_kstr = iobj_1 -> Type -> size [ 0 ] * iobj_1 -> Type -> size [ 1 ] ;
iobj_1 -> Type -> size [ 0 ] = 1 ; iobj_1 -> Type -> size [ 1 ] = 5 ;
e3n3mnwk4b ( iobj_1 -> Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ;
b_kstr ++ ) { iobj_1 -> Type -> data [ b_kstr ] = tmp_m [ b_kstr ] ; }
m2n1niwoq4 ( & switch_expression , 2 ) ; b_kstr = switch_expression -> size [
0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = iobj_1 -> Type -> size [ 1 ] ; e3n3mnwk4b (
switch_expression , b_kstr ) ; loop_ub = iobj_1 -> Type -> size [ 1 ] ; for (
b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [
b_kstr ] = iobj_1 -> Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr <
8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_g [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_j [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ]
= 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } poslim_data [ 0 ] = - 3.1415926535897931 ; poslim_data [ 1 ] =
3.1415926535897931 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; case 1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] =
0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++
) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } poslim_data [ 0 ] = - 0.5
; poslim_data [ 1 ] = 0.5 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } poslim_data [ 0 ] = 0.0 ; poslim_data [ 1 ]
= 0.0 ; iobj_1 -> VelocityNumber = 0.0 ; iobj_1 -> PositionNumber = 0.0 ;
iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 1 ] =
0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = iobj_1 ->
MotionSubspace -> size [ 0 ] * iobj_1 -> MotionSubspace -> size [ 1 ] ;
iobj_1 -> MotionSubspace -> size [ 0 ] = 6 ; iobj_1 -> MotionSubspace -> size
[ 1 ] = 1 ; miyl1rq4uj ( iobj_1 -> MotionSubspace , b_kstr ) ; for ( b_kstr =
0 ; b_kstr < 6 ; b_kstr ++ ) { iobj_1 -> MotionSubspace -> data [ b_kstr ] =
msubspace_data [ b_kstr ] ; } b_kstr = iobj_1 -> PositionLimitsInternal ->
size [ 0 ] * iobj_1 -> PositionLimitsInternal -> size [ 1 ] ; iobj_1 ->
PositionLimitsInternal -> size [ 0 ] = 1 ; iobj_1 -> PositionLimitsInternal
-> size [ 1 ] = 2 ; miyl1rq4uj ( iobj_1 -> PositionLimitsInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 2 ; b_kstr ++ ) { iobj_1 ->
PositionLimitsInternal -> data [ b_kstr ] = poslim_data [ b_kstr ] ; } b_kstr
= iobj_1 -> HomePositionInternal -> size [ 0 ] ; iobj_1 ->
HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( iobj_1 ->
HomePositionInternal , b_kstr ) ; iobj_1 -> HomePositionInternal -> data [ 0
] = 0.0 ; obj -> JointInternal = iobj_1 ; obj -> Index = - 1.0 ; obj ->
ParentIndex = - 1.0 ; obj -> MassInternal = 1.0 ; obj -> CenterOfMassInternal
[ 0 ] = 0.0 ; obj -> CenterOfMassInternal [ 1 ] = 0.0 ; obj ->
CenterOfMassInternal [ 2 ] = 0.0 ; for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++
) { b_I [ b_kstr ] = 0 ; } b_I [ 0 ] = 1 ; b_I [ 4 ] = 1 ; b_I [ 8 ] = 1 ;
for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { obj -> InertiaInternal [ b_kstr
] = b_I [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ )
{ msubspace_data [ b_kstr + 6 * b_kstr ] = 1 ; } for ( b_kstr = 0 ; b_kstr <
36 ; b_kstr ++ ) { obj -> SpatialInertia [ b_kstr ] = msubspace_data [ b_kstr
] ; } obj -> CollisionsInternal = p1kacjwirim ( iobj_0 , 0.0 ) ; obj ->
matlabCodegenIsDeleted = false ; return b_obj ; } static alyuhn31fbfz *
e0kx4oauic ( alyuhn31fbfz * obj , ogx4llc4arl * iobj_0 , ot4uovzkt0w * iobj_1
) { alyuhn31fbfz * b_obj ; e1n154mvbh * switch_expression ; real_T
poslim_data [ 12 ] ; int32_T b_kstr ; int32_T loop_ub ; char_T b_p [ 9 ] ;
char_T b [ 8 ] ; int8_T msubspace_data [ 36 ] ; int8_T b_I [ 9 ] ; int8_T tmp
[ 6 ] ; boolean_T b_bool ; static const char_T tmp_p [ 10 ] = { 'd' , 'u' ,
'm' , 'm' , 'y' , 'b' , 'o' , 'd' , 'y' , '2' } ; static const int8_T tmp_e [
16 ] = { 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 } ;
static const char_T tmp_i [ 14 ] = { 'd' , 'u' , 'm' , 'm' , 'y' , 'b' , 'o'
, 'd' , 'y' , '2' , '_' , 'j' , 'n' , 't' } ; static const char_T tmp_m [ 5 ]
= { 'f' , 'i' , 'x' , 'e' , 'd' } ; static const char_T tmp_g [ 8 ] = { 'r' ,
'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ; static const char_T tmp_j [ 9 ] =
{ 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' , 'i' , 'c' } ; int32_T exitg1 ;
b_obj = obj ; b_kstr = obj -> NameInternal -> size [ 0 ] * obj ->
NameInternal -> size [ 1 ] ; obj -> NameInternal -> size [ 0 ] = 1 ; obj ->
NameInternal -> size [ 1 ] = 10 ; e3n3mnwk4b ( obj -> NameInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 10 ; b_kstr ++ ) { obj -> NameInternal -> data
[ b_kstr ] = tmp_p [ b_kstr ] ; } iobj_1 -> InTree = false ; for ( b_kstr = 0
; b_kstr < 16 ; b_kstr ++ ) { iobj_1 -> JointToParentTransform [ b_kstr ] =
tmp_e [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { iobj_1 ->
ChildToJointTransform [ b_kstr ] = tmp_e [ b_kstr ] ; } b_kstr = iobj_1 ->
NameInternal -> size [ 0 ] * iobj_1 -> NameInternal -> size [ 1 ] ; iobj_1 ->
NameInternal -> size [ 0 ] = 1 ; iobj_1 -> NameInternal -> size [ 1 ] = 14 ;
e3n3mnwk4b ( iobj_1 -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr <
14 ; b_kstr ++ ) { iobj_1 -> NameInternal -> data [ b_kstr ] = tmp_i [ b_kstr
] ; } b_kstr = iobj_1 -> Type -> size [ 0 ] * iobj_1 -> Type -> size [ 1 ] ;
iobj_1 -> Type -> size [ 0 ] = 1 ; iobj_1 -> Type -> size [ 1 ] = 5 ;
e3n3mnwk4b ( iobj_1 -> Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ;
b_kstr ++ ) { iobj_1 -> Type -> data [ b_kstr ] = tmp_m [ b_kstr ] ; }
m2n1niwoq4 ( & switch_expression , 2 ) ; b_kstr = switch_expression -> size [
0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = iobj_1 -> Type -> size [ 1 ] ; e3n3mnwk4b (
switch_expression , b_kstr ) ; loop_ub = iobj_1 -> Type -> size [ 1 ] ; for (
b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [
b_kstr ] = iobj_1 -> Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr <
8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_g [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_j [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ]
= 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } poslim_data [ 0 ] = - 3.1415926535897931 ; poslim_data [ 1 ] =
3.1415926535897931 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; case 1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] =
0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++
) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } poslim_data [ 0 ] = - 0.5
; poslim_data [ 1 ] = 0.5 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } poslim_data [ 0 ] = 0.0 ; poslim_data [ 1 ]
= 0.0 ; iobj_1 -> VelocityNumber = 0.0 ; iobj_1 -> PositionNumber = 0.0 ;
iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 1 ] =
0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = iobj_1 ->
MotionSubspace -> size [ 0 ] * iobj_1 -> MotionSubspace -> size [ 1 ] ;
iobj_1 -> MotionSubspace -> size [ 0 ] = 6 ; iobj_1 -> MotionSubspace -> size
[ 1 ] = 1 ; miyl1rq4uj ( iobj_1 -> MotionSubspace , b_kstr ) ; for ( b_kstr =
0 ; b_kstr < 6 ; b_kstr ++ ) { iobj_1 -> MotionSubspace -> data [ b_kstr ] =
msubspace_data [ b_kstr ] ; } b_kstr = iobj_1 -> PositionLimitsInternal ->
size [ 0 ] * iobj_1 -> PositionLimitsInternal -> size [ 1 ] ; iobj_1 ->
PositionLimitsInternal -> size [ 0 ] = 1 ; iobj_1 -> PositionLimitsInternal
-> size [ 1 ] = 2 ; miyl1rq4uj ( iobj_1 -> PositionLimitsInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 2 ; b_kstr ++ ) { iobj_1 ->
PositionLimitsInternal -> data [ b_kstr ] = poslim_data [ b_kstr ] ; } b_kstr
= iobj_1 -> HomePositionInternal -> size [ 0 ] ; iobj_1 ->
HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( iobj_1 ->
HomePositionInternal , b_kstr ) ; iobj_1 -> HomePositionInternal -> data [ 0
] = 0.0 ; obj -> JointInternal = iobj_1 ; obj -> Index = - 1.0 ; obj ->
ParentIndex = - 1.0 ; obj -> MassInternal = 1.0 ; obj -> CenterOfMassInternal
[ 0 ] = 0.0 ; obj -> CenterOfMassInternal [ 1 ] = 0.0 ; obj ->
CenterOfMassInternal [ 2 ] = 0.0 ; for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++
) { b_I [ b_kstr ] = 0 ; } b_I [ 0 ] = 1 ; b_I [ 4 ] = 1 ; b_I [ 8 ] = 1 ;
for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { obj -> InertiaInternal [ b_kstr
] = b_I [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ )
{ msubspace_data [ b_kstr + 6 * b_kstr ] = 1 ; } for ( b_kstr = 0 ; b_kstr <
36 ; b_kstr ++ ) { obj -> SpatialInertia [ b_kstr ] = msubspace_data [ b_kstr
] ; } obj -> CollisionsInternal = p1kacjwirim ( iobj_0 , 0.0 ) ; obj ->
matlabCodegenIsDeleted = false ; return b_obj ; } static alyuhn31fbfz *
mpzyhljwzq ( alyuhn31fbfz * obj , ogx4llc4arl * iobj_0 , ot4uovzkt0w * iobj_1
) { alyuhn31fbfz * b_obj ; e1n154mvbh * switch_expression ; real_T
poslim_data [ 12 ] ; int32_T b_kstr ; int32_T loop_ub ; char_T b_p [ 9 ] ;
char_T b [ 8 ] ; int8_T msubspace_data [ 36 ] ; int8_T b_I [ 9 ] ; int8_T tmp
[ 6 ] ; boolean_T b_bool ; static const char_T tmp_p [ 10 ] = { 'd' , 'u' ,
'm' , 'm' , 'y' , 'b' , 'o' , 'd' , 'y' , '3' } ; static const int8_T tmp_e [
16 ] = { 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 } ;
static const char_T tmp_i [ 14 ] = { 'd' , 'u' , 'm' , 'm' , 'y' , 'b' , 'o'
, 'd' , 'y' , '3' , '_' , 'j' , 'n' , 't' } ; static const char_T tmp_m [ 5 ]
= { 'f' , 'i' , 'x' , 'e' , 'd' } ; static const char_T tmp_g [ 8 ] = { 'r' ,
'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ; static const char_T tmp_j [ 9 ] =
{ 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' , 'i' , 'c' } ; int32_T exitg1 ;
b_obj = obj ; b_kstr = obj -> NameInternal -> size [ 0 ] * obj ->
NameInternal -> size [ 1 ] ; obj -> NameInternal -> size [ 0 ] = 1 ; obj ->
NameInternal -> size [ 1 ] = 10 ; e3n3mnwk4b ( obj -> NameInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 10 ; b_kstr ++ ) { obj -> NameInternal -> data
[ b_kstr ] = tmp_p [ b_kstr ] ; } iobj_1 -> InTree = false ; for ( b_kstr = 0
; b_kstr < 16 ; b_kstr ++ ) { iobj_1 -> JointToParentTransform [ b_kstr ] =
tmp_e [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { iobj_1 ->
ChildToJointTransform [ b_kstr ] = tmp_e [ b_kstr ] ; } b_kstr = iobj_1 ->
NameInternal -> size [ 0 ] * iobj_1 -> NameInternal -> size [ 1 ] ; iobj_1 ->
NameInternal -> size [ 0 ] = 1 ; iobj_1 -> NameInternal -> size [ 1 ] = 14 ;
e3n3mnwk4b ( iobj_1 -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr <
14 ; b_kstr ++ ) { iobj_1 -> NameInternal -> data [ b_kstr ] = tmp_i [ b_kstr
] ; } b_kstr = iobj_1 -> Type -> size [ 0 ] * iobj_1 -> Type -> size [ 1 ] ;
iobj_1 -> Type -> size [ 0 ] = 1 ; iobj_1 -> Type -> size [ 1 ] = 5 ;
e3n3mnwk4b ( iobj_1 -> Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ;
b_kstr ++ ) { iobj_1 -> Type -> data [ b_kstr ] = tmp_m [ b_kstr ] ; }
m2n1niwoq4 ( & switch_expression , 2 ) ; b_kstr = switch_expression -> size [
0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = iobj_1 -> Type -> size [ 1 ] ; e3n3mnwk4b (
switch_expression , b_kstr ) ; loop_ub = iobj_1 -> Type -> size [ 1 ] ; for (
b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [
b_kstr ] = iobj_1 -> Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr <
8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_g [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_j [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ]
= 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } poslim_data [ 0 ] = - 3.1415926535897931 ; poslim_data [ 1 ] =
3.1415926535897931 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; case 1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] =
0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++
) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } poslim_data [ 0 ] = - 0.5
; poslim_data [ 1 ] = 0.5 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } poslim_data [ 0 ] = 0.0 ; poslim_data [ 1 ]
= 0.0 ; iobj_1 -> VelocityNumber = 0.0 ; iobj_1 -> PositionNumber = 0.0 ;
iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 1 ] =
0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = iobj_1 ->
MotionSubspace -> size [ 0 ] * iobj_1 -> MotionSubspace -> size [ 1 ] ;
iobj_1 -> MotionSubspace -> size [ 0 ] = 6 ; iobj_1 -> MotionSubspace -> size
[ 1 ] = 1 ; miyl1rq4uj ( iobj_1 -> MotionSubspace , b_kstr ) ; for ( b_kstr =
0 ; b_kstr < 6 ; b_kstr ++ ) { iobj_1 -> MotionSubspace -> data [ b_kstr ] =
msubspace_data [ b_kstr ] ; } b_kstr = iobj_1 -> PositionLimitsInternal ->
size [ 0 ] * iobj_1 -> PositionLimitsInternal -> size [ 1 ] ; iobj_1 ->
PositionLimitsInternal -> size [ 0 ] = 1 ; iobj_1 -> PositionLimitsInternal
-> size [ 1 ] = 2 ; miyl1rq4uj ( iobj_1 -> PositionLimitsInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 2 ; b_kstr ++ ) { iobj_1 ->
PositionLimitsInternal -> data [ b_kstr ] = poslim_data [ b_kstr ] ; } b_kstr
= iobj_1 -> HomePositionInternal -> size [ 0 ] ; iobj_1 ->
HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( iobj_1 ->
HomePositionInternal , b_kstr ) ; iobj_1 -> HomePositionInternal -> data [ 0
] = 0.0 ; obj -> JointInternal = iobj_1 ; obj -> Index = - 1.0 ; obj ->
ParentIndex = - 1.0 ; obj -> MassInternal = 1.0 ; obj -> CenterOfMassInternal
[ 0 ] = 0.0 ; obj -> CenterOfMassInternal [ 1 ] = 0.0 ; obj ->
CenterOfMassInternal [ 2 ] = 0.0 ; for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++
) { b_I [ b_kstr ] = 0 ; } b_I [ 0 ] = 1 ; b_I [ 4 ] = 1 ; b_I [ 8 ] = 1 ;
for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { obj -> InertiaInternal [ b_kstr
] = b_I [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ )
{ msubspace_data [ b_kstr + 6 * b_kstr ] = 1 ; } for ( b_kstr = 0 ; b_kstr <
36 ; b_kstr ++ ) { obj -> SpatialInertia [ b_kstr ] = msubspace_data [ b_kstr
] ; } obj -> CollisionsInternal = p1kacjwirim ( iobj_0 , 0.0 ) ; obj ->
matlabCodegenIsDeleted = false ; return b_obj ; } static alyuhn31fbfz *
axww4prr0o ( alyuhn31fbfz * obj , ogx4llc4arl * iobj_0 , ot4uovzkt0w * iobj_1
) { alyuhn31fbfz * b_obj ; e1n154mvbh * switch_expression ; real_T
poslim_data [ 12 ] ; int32_T b_kstr ; int32_T loop_ub ; char_T b_p [ 9 ] ;
char_T b [ 8 ] ; int8_T msubspace_data [ 36 ] ; int8_T b_I [ 9 ] ; int8_T tmp
[ 6 ] ; boolean_T b_bool ; static const char_T tmp_p [ 10 ] = { 'd' , 'u' ,
'm' , 'm' , 'y' , 'b' , 'o' , 'd' , 'y' , '4' } ; static const int8_T tmp_e [
16 ] = { 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 } ;
static const char_T tmp_i [ 14 ] = { 'd' , 'u' , 'm' , 'm' , 'y' , 'b' , 'o'
, 'd' , 'y' , '4' , '_' , 'j' , 'n' , 't' } ; static const char_T tmp_m [ 5 ]
= { 'f' , 'i' , 'x' , 'e' , 'd' } ; static const char_T tmp_g [ 8 ] = { 'r' ,
'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ; static const char_T tmp_j [ 9 ] =
{ 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' , 'i' , 'c' } ; int32_T exitg1 ;
b_obj = obj ; b_kstr = obj -> NameInternal -> size [ 0 ] * obj ->
NameInternal -> size [ 1 ] ; obj -> NameInternal -> size [ 0 ] = 1 ; obj ->
NameInternal -> size [ 1 ] = 10 ; e3n3mnwk4b ( obj -> NameInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 10 ; b_kstr ++ ) { obj -> NameInternal -> data
[ b_kstr ] = tmp_p [ b_kstr ] ; } iobj_1 -> InTree = false ; for ( b_kstr = 0
; b_kstr < 16 ; b_kstr ++ ) { iobj_1 -> JointToParentTransform [ b_kstr ] =
tmp_e [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { iobj_1 ->
ChildToJointTransform [ b_kstr ] = tmp_e [ b_kstr ] ; } b_kstr = iobj_1 ->
NameInternal -> size [ 0 ] * iobj_1 -> NameInternal -> size [ 1 ] ; iobj_1 ->
NameInternal -> size [ 0 ] = 1 ; iobj_1 -> NameInternal -> size [ 1 ] = 14 ;
e3n3mnwk4b ( iobj_1 -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr <
14 ; b_kstr ++ ) { iobj_1 -> NameInternal -> data [ b_kstr ] = tmp_i [ b_kstr
] ; } b_kstr = iobj_1 -> Type -> size [ 0 ] * iobj_1 -> Type -> size [ 1 ] ;
iobj_1 -> Type -> size [ 0 ] = 1 ; iobj_1 -> Type -> size [ 1 ] = 5 ;
e3n3mnwk4b ( iobj_1 -> Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ;
b_kstr ++ ) { iobj_1 -> Type -> data [ b_kstr ] = tmp_m [ b_kstr ] ; }
m2n1niwoq4 ( & switch_expression , 2 ) ; b_kstr = switch_expression -> size [
0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = iobj_1 -> Type -> size [ 1 ] ; e3n3mnwk4b (
switch_expression , b_kstr ) ; loop_ub = iobj_1 -> Type -> size [ 1 ] ; for (
b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [
b_kstr ] = iobj_1 -> Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr <
8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_g [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_j [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ]
= 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } poslim_data [ 0 ] = - 3.1415926535897931 ; poslim_data [ 1 ] =
3.1415926535897931 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; case 1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] =
0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++
) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } poslim_data [ 0 ] = - 0.5
; poslim_data [ 1 ] = 0.5 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } poslim_data [ 0 ] = 0.0 ; poslim_data [ 1 ]
= 0.0 ; iobj_1 -> VelocityNumber = 0.0 ; iobj_1 -> PositionNumber = 0.0 ;
iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 1 ] =
0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = iobj_1 ->
MotionSubspace -> size [ 0 ] * iobj_1 -> MotionSubspace -> size [ 1 ] ;
iobj_1 -> MotionSubspace -> size [ 0 ] = 6 ; iobj_1 -> MotionSubspace -> size
[ 1 ] = 1 ; miyl1rq4uj ( iobj_1 -> MotionSubspace , b_kstr ) ; for ( b_kstr =
0 ; b_kstr < 6 ; b_kstr ++ ) { iobj_1 -> MotionSubspace -> data [ b_kstr ] =
msubspace_data [ b_kstr ] ; } b_kstr = iobj_1 -> PositionLimitsInternal ->
size [ 0 ] * iobj_1 -> PositionLimitsInternal -> size [ 1 ] ; iobj_1 ->
PositionLimitsInternal -> size [ 0 ] = 1 ; iobj_1 -> PositionLimitsInternal
-> size [ 1 ] = 2 ; miyl1rq4uj ( iobj_1 -> PositionLimitsInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 2 ; b_kstr ++ ) { iobj_1 ->
PositionLimitsInternal -> data [ b_kstr ] = poslim_data [ b_kstr ] ; } b_kstr
= iobj_1 -> HomePositionInternal -> size [ 0 ] ; iobj_1 ->
HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( iobj_1 ->
HomePositionInternal , b_kstr ) ; iobj_1 -> HomePositionInternal -> data [ 0
] = 0.0 ; obj -> JointInternal = iobj_1 ; obj -> Index = - 1.0 ; obj ->
ParentIndex = - 1.0 ; obj -> MassInternal = 1.0 ; obj -> CenterOfMassInternal
[ 0 ] = 0.0 ; obj -> CenterOfMassInternal [ 1 ] = 0.0 ; obj ->
CenterOfMassInternal [ 2 ] = 0.0 ; for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++
) { b_I [ b_kstr ] = 0 ; } b_I [ 0 ] = 1 ; b_I [ 4 ] = 1 ; b_I [ 8 ] = 1 ;
for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { obj -> InertiaInternal [ b_kstr
] = b_I [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ )
{ msubspace_data [ b_kstr + 6 * b_kstr ] = 1 ; } for ( b_kstr = 0 ; b_kstr <
36 ; b_kstr ++ ) { obj -> SpatialInertia [ b_kstr ] = msubspace_data [ b_kstr
] ; } obj -> CollisionsInternal = p1kacjwirim ( iobj_0 , 0.0 ) ; obj ->
matlabCodegenIsDeleted = false ; return b_obj ; } static alyuhn31fbfz *
oh2xpcdbml ( alyuhn31fbfz * obj , ogx4llc4arl * iobj_0 , ot4uovzkt0w * iobj_1
) { alyuhn31fbfz * b_obj ; e1n154mvbh * switch_expression ; real_T
poslim_data [ 12 ] ; int32_T b_kstr ; int32_T loop_ub ; char_T b_p [ 9 ] ;
char_T b [ 8 ] ; int8_T msubspace_data [ 36 ] ; int8_T b_I [ 9 ] ; int8_T tmp
[ 6 ] ; boolean_T b_bool ; static const char_T tmp_p [ 10 ] = { 'd' , 'u' ,
'm' , 'm' , 'y' , 'b' , 'o' , 'd' , 'y' , '5' } ; static const int8_T tmp_e [
16 ] = { 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 } ;
static const char_T tmp_i [ 14 ] = { 'd' , 'u' , 'm' , 'm' , 'y' , 'b' , 'o'
, 'd' , 'y' , '5' , '_' , 'j' , 'n' , 't' } ; static const char_T tmp_m [ 5 ]
= { 'f' , 'i' , 'x' , 'e' , 'd' } ; static const char_T tmp_g [ 8 ] = { 'r' ,
'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ; static const char_T tmp_j [ 9 ] =
{ 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' , 'i' , 'c' } ; int32_T exitg1 ;
b_obj = obj ; b_kstr = obj -> NameInternal -> size [ 0 ] * obj ->
NameInternal -> size [ 1 ] ; obj -> NameInternal -> size [ 0 ] = 1 ; obj ->
NameInternal -> size [ 1 ] = 10 ; e3n3mnwk4b ( obj -> NameInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 10 ; b_kstr ++ ) { obj -> NameInternal -> data
[ b_kstr ] = tmp_p [ b_kstr ] ; } iobj_1 -> InTree = false ; for ( b_kstr = 0
; b_kstr < 16 ; b_kstr ++ ) { iobj_1 -> JointToParentTransform [ b_kstr ] =
tmp_e [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { iobj_1 ->
ChildToJointTransform [ b_kstr ] = tmp_e [ b_kstr ] ; } b_kstr = iobj_1 ->
NameInternal -> size [ 0 ] * iobj_1 -> NameInternal -> size [ 1 ] ; iobj_1 ->
NameInternal -> size [ 0 ] = 1 ; iobj_1 -> NameInternal -> size [ 1 ] = 14 ;
e3n3mnwk4b ( iobj_1 -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr <
14 ; b_kstr ++ ) { iobj_1 -> NameInternal -> data [ b_kstr ] = tmp_i [ b_kstr
] ; } b_kstr = iobj_1 -> Type -> size [ 0 ] * iobj_1 -> Type -> size [ 1 ] ;
iobj_1 -> Type -> size [ 0 ] = 1 ; iobj_1 -> Type -> size [ 1 ] = 5 ;
e3n3mnwk4b ( iobj_1 -> Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ;
b_kstr ++ ) { iobj_1 -> Type -> data [ b_kstr ] = tmp_m [ b_kstr ] ; }
m2n1niwoq4 ( & switch_expression , 2 ) ; b_kstr = switch_expression -> size [
0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = iobj_1 -> Type -> size [ 1 ] ; e3n3mnwk4b (
switch_expression , b_kstr ) ; loop_ub = iobj_1 -> Type -> size [ 1 ] ; for (
b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [
b_kstr ] = iobj_1 -> Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr <
8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_g [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_j [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ]
= 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } poslim_data [ 0 ] = - 3.1415926535897931 ; poslim_data [ 1 ] =
3.1415926535897931 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; case 1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] =
0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++
) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } poslim_data [ 0 ] = - 0.5
; poslim_data [ 1 ] = 0.5 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } poslim_data [ 0 ] = 0.0 ; poslim_data [ 1 ]
= 0.0 ; iobj_1 -> VelocityNumber = 0.0 ; iobj_1 -> PositionNumber = 0.0 ;
iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 1 ] =
0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = iobj_1 ->
MotionSubspace -> size [ 0 ] * iobj_1 -> MotionSubspace -> size [ 1 ] ;
iobj_1 -> MotionSubspace -> size [ 0 ] = 6 ; iobj_1 -> MotionSubspace -> size
[ 1 ] = 1 ; miyl1rq4uj ( iobj_1 -> MotionSubspace , b_kstr ) ; for ( b_kstr =
0 ; b_kstr < 6 ; b_kstr ++ ) { iobj_1 -> MotionSubspace -> data [ b_kstr ] =
msubspace_data [ b_kstr ] ; } b_kstr = iobj_1 -> PositionLimitsInternal ->
size [ 0 ] * iobj_1 -> PositionLimitsInternal -> size [ 1 ] ; iobj_1 ->
PositionLimitsInternal -> size [ 0 ] = 1 ; iobj_1 -> PositionLimitsInternal
-> size [ 1 ] = 2 ; miyl1rq4uj ( iobj_1 -> PositionLimitsInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 2 ; b_kstr ++ ) { iobj_1 ->
PositionLimitsInternal -> data [ b_kstr ] = poslim_data [ b_kstr ] ; } b_kstr
= iobj_1 -> HomePositionInternal -> size [ 0 ] ; iobj_1 ->
HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( iobj_1 ->
HomePositionInternal , b_kstr ) ; iobj_1 -> HomePositionInternal -> data [ 0
] = 0.0 ; obj -> JointInternal = iobj_1 ; obj -> Index = - 1.0 ; obj ->
ParentIndex = - 1.0 ; obj -> MassInternal = 1.0 ; obj -> CenterOfMassInternal
[ 0 ] = 0.0 ; obj -> CenterOfMassInternal [ 1 ] = 0.0 ; obj ->
CenterOfMassInternal [ 2 ] = 0.0 ; for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++
) { b_I [ b_kstr ] = 0 ; } b_I [ 0 ] = 1 ; b_I [ 4 ] = 1 ; b_I [ 8 ] = 1 ;
for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { obj -> InertiaInternal [ b_kstr
] = b_I [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ )
{ msubspace_data [ b_kstr + 6 * b_kstr ] = 1 ; } for ( b_kstr = 0 ; b_kstr <
36 ; b_kstr ++ ) { obj -> SpatialInertia [ b_kstr ] = msubspace_data [ b_kstr
] ; } obj -> CollisionsInternal = p1kacjwirim ( iobj_0 , 0.0 ) ; obj ->
matlabCodegenIsDeleted = false ; return b_obj ; } static alyuhn31fbfz *
hcmkep3fjc ( alyuhn31fbfz * obj , ogx4llc4arl * iobj_0 , ot4uovzkt0w * iobj_1
) { alyuhn31fbfz * b_obj ; e1n154mvbh * switch_expression ; real_T
poslim_data [ 12 ] ; int32_T b_kstr ; int32_T loop_ub ; char_T b_p [ 9 ] ;
char_T b [ 8 ] ; int8_T msubspace_data [ 36 ] ; int8_T b_I [ 9 ] ; int8_T tmp
[ 6 ] ; boolean_T b_bool ; static const char_T tmp_p [ 10 ] = { 'd' , 'u' ,
'm' , 'm' , 'y' , 'b' , 'o' , 'd' , 'y' , '6' } ; static const int8_T tmp_e [
16 ] = { 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 } ;
static const char_T tmp_i [ 14 ] = { 'd' , 'u' , 'm' , 'm' , 'y' , 'b' , 'o'
, 'd' , 'y' , '6' , '_' , 'j' , 'n' , 't' } ; static const char_T tmp_m [ 5 ]
= { 'f' , 'i' , 'x' , 'e' , 'd' } ; static const char_T tmp_g [ 8 ] = { 'r' ,
'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ; static const char_T tmp_j [ 9 ] =
{ 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' , 'i' , 'c' } ; int32_T exitg1 ;
b_obj = obj ; b_kstr = obj -> NameInternal -> size [ 0 ] * obj ->
NameInternal -> size [ 1 ] ; obj -> NameInternal -> size [ 0 ] = 1 ; obj ->
NameInternal -> size [ 1 ] = 10 ; e3n3mnwk4b ( obj -> NameInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 10 ; b_kstr ++ ) { obj -> NameInternal -> data
[ b_kstr ] = tmp_p [ b_kstr ] ; } iobj_1 -> InTree = false ; for ( b_kstr = 0
; b_kstr < 16 ; b_kstr ++ ) { iobj_1 -> JointToParentTransform [ b_kstr ] =
tmp_e [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { iobj_1 ->
ChildToJointTransform [ b_kstr ] = tmp_e [ b_kstr ] ; } b_kstr = iobj_1 ->
NameInternal -> size [ 0 ] * iobj_1 -> NameInternal -> size [ 1 ] ; iobj_1 ->
NameInternal -> size [ 0 ] = 1 ; iobj_1 -> NameInternal -> size [ 1 ] = 14 ;
e3n3mnwk4b ( iobj_1 -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr <
14 ; b_kstr ++ ) { iobj_1 -> NameInternal -> data [ b_kstr ] = tmp_i [ b_kstr
] ; } b_kstr = iobj_1 -> Type -> size [ 0 ] * iobj_1 -> Type -> size [ 1 ] ;
iobj_1 -> Type -> size [ 0 ] = 1 ; iobj_1 -> Type -> size [ 1 ] = 5 ;
e3n3mnwk4b ( iobj_1 -> Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ;
b_kstr ++ ) { iobj_1 -> Type -> data [ b_kstr ] = tmp_m [ b_kstr ] ; }
m2n1niwoq4 ( & switch_expression , 2 ) ; b_kstr = switch_expression -> size [
0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = iobj_1 -> Type -> size [ 1 ] ; e3n3mnwk4b (
switch_expression , b_kstr ) ; loop_ub = iobj_1 -> Type -> size [ 1 ] ; for (
b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [
b_kstr ] = iobj_1 -> Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr <
8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_g [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_j [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ]
= 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } poslim_data [ 0 ] = - 3.1415926535897931 ; poslim_data [ 1 ] =
3.1415926535897931 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; case 1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] =
0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++
) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } poslim_data [ 0 ] = - 0.5
; poslim_data [ 1 ] = 0.5 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } poslim_data [ 0 ] = 0.0 ; poslim_data [ 1 ]
= 0.0 ; iobj_1 -> VelocityNumber = 0.0 ; iobj_1 -> PositionNumber = 0.0 ;
iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 1 ] =
0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = iobj_1 ->
MotionSubspace -> size [ 0 ] * iobj_1 -> MotionSubspace -> size [ 1 ] ;
iobj_1 -> MotionSubspace -> size [ 0 ] = 6 ; iobj_1 -> MotionSubspace -> size
[ 1 ] = 1 ; miyl1rq4uj ( iobj_1 -> MotionSubspace , b_kstr ) ; for ( b_kstr =
0 ; b_kstr < 6 ; b_kstr ++ ) { iobj_1 -> MotionSubspace -> data [ b_kstr ] =
msubspace_data [ b_kstr ] ; } b_kstr = iobj_1 -> PositionLimitsInternal ->
size [ 0 ] * iobj_1 -> PositionLimitsInternal -> size [ 1 ] ; iobj_1 ->
PositionLimitsInternal -> size [ 0 ] = 1 ; iobj_1 -> PositionLimitsInternal
-> size [ 1 ] = 2 ; miyl1rq4uj ( iobj_1 -> PositionLimitsInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 2 ; b_kstr ++ ) { iobj_1 ->
PositionLimitsInternal -> data [ b_kstr ] = poslim_data [ b_kstr ] ; } b_kstr
= iobj_1 -> HomePositionInternal -> size [ 0 ] ; iobj_1 ->
HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( iobj_1 ->
HomePositionInternal , b_kstr ) ; iobj_1 -> HomePositionInternal -> data [ 0
] = 0.0 ; obj -> JointInternal = iobj_1 ; obj -> Index = - 1.0 ; obj ->
ParentIndex = - 1.0 ; obj -> MassInternal = 1.0 ; obj -> CenterOfMassInternal
[ 0 ] = 0.0 ; obj -> CenterOfMassInternal [ 1 ] = 0.0 ; obj ->
CenterOfMassInternal [ 2 ] = 0.0 ; for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++
) { b_I [ b_kstr ] = 0 ; } b_I [ 0 ] = 1 ; b_I [ 4 ] = 1 ; b_I [ 8 ] = 1 ;
for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { obj -> InertiaInternal [ b_kstr
] = b_I [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ )
{ msubspace_data [ b_kstr + 6 * b_kstr ] = 1 ; } for ( b_kstr = 0 ; b_kstr <
36 ; b_kstr ++ ) { obj -> SpatialInertia [ b_kstr ] = msubspace_data [ b_kstr
] ; } obj -> CollisionsInternal = p1kacjwirim ( iobj_0 , 0.0 ) ; obj ->
matlabCodegenIsDeleted = false ; return b_obj ; } static alyuhn31fbfz *
eearo4zkrq ( alyuhn31fbfz * obj , ogx4llc4arl * iobj_0 , ot4uovzkt0w * iobj_1
) { alyuhn31fbfz * b_obj ; e1n154mvbh * switch_expression ; real_T
poslim_data [ 12 ] ; int32_T b_kstr ; int32_T loop_ub ; char_T b_p [ 9 ] ;
char_T b [ 8 ] ; int8_T msubspace_data [ 36 ] ; int8_T b_I [ 9 ] ; int8_T tmp
[ 6 ] ; boolean_T b_bool ; static const char_T tmp_p [ 10 ] = { 'd' , 'u' ,
'm' , 'm' , 'y' , 'b' , 'o' , 'd' , 'y' , '7' } ; static const int8_T tmp_e [
16 ] = { 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 } ;
static const char_T tmp_i [ 14 ] = { 'd' , 'u' , 'm' , 'm' , 'y' , 'b' , 'o'
, 'd' , 'y' , '7' , '_' , 'j' , 'n' , 't' } ; static const char_T tmp_m [ 5 ]
= { 'f' , 'i' , 'x' , 'e' , 'd' } ; static const char_T tmp_g [ 8 ] = { 'r' ,
'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ; static const char_T tmp_j [ 9 ] =
{ 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' , 'i' , 'c' } ; int32_T exitg1 ;
b_obj = obj ; b_kstr = obj -> NameInternal -> size [ 0 ] * obj ->
NameInternal -> size [ 1 ] ; obj -> NameInternal -> size [ 0 ] = 1 ; obj ->
NameInternal -> size [ 1 ] = 10 ; e3n3mnwk4b ( obj -> NameInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 10 ; b_kstr ++ ) { obj -> NameInternal -> data
[ b_kstr ] = tmp_p [ b_kstr ] ; } iobj_1 -> InTree = false ; for ( b_kstr = 0
; b_kstr < 16 ; b_kstr ++ ) { iobj_1 -> JointToParentTransform [ b_kstr ] =
tmp_e [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { iobj_1 ->
ChildToJointTransform [ b_kstr ] = tmp_e [ b_kstr ] ; } b_kstr = iobj_1 ->
NameInternal -> size [ 0 ] * iobj_1 -> NameInternal -> size [ 1 ] ; iobj_1 ->
NameInternal -> size [ 0 ] = 1 ; iobj_1 -> NameInternal -> size [ 1 ] = 14 ;
e3n3mnwk4b ( iobj_1 -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr <
14 ; b_kstr ++ ) { iobj_1 -> NameInternal -> data [ b_kstr ] = tmp_i [ b_kstr
] ; } b_kstr = iobj_1 -> Type -> size [ 0 ] * iobj_1 -> Type -> size [ 1 ] ;
iobj_1 -> Type -> size [ 0 ] = 1 ; iobj_1 -> Type -> size [ 1 ] = 5 ;
e3n3mnwk4b ( iobj_1 -> Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ;
b_kstr ++ ) { iobj_1 -> Type -> data [ b_kstr ] = tmp_m [ b_kstr ] ; }
m2n1niwoq4 ( & switch_expression , 2 ) ; b_kstr = switch_expression -> size [
0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = iobj_1 -> Type -> size [ 1 ] ; e3n3mnwk4b (
switch_expression , b_kstr ) ; loop_ub = iobj_1 -> Type -> size [ 1 ] ; for (
b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [
b_kstr ] = iobj_1 -> Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr <
8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_g [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_j [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ]
= 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } poslim_data [ 0 ] = - 3.1415926535897931 ; poslim_data [ 1 ] =
3.1415926535897931 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; case 1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] =
0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++
) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } poslim_data [ 0 ] = - 0.5
; poslim_data [ 1 ] = 0.5 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } poslim_data [ 0 ] = 0.0 ; poslim_data [ 1 ]
= 0.0 ; iobj_1 -> VelocityNumber = 0.0 ; iobj_1 -> PositionNumber = 0.0 ;
iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 1 ] =
0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = iobj_1 ->
MotionSubspace -> size [ 0 ] * iobj_1 -> MotionSubspace -> size [ 1 ] ;
iobj_1 -> MotionSubspace -> size [ 0 ] = 6 ; iobj_1 -> MotionSubspace -> size
[ 1 ] = 1 ; miyl1rq4uj ( iobj_1 -> MotionSubspace , b_kstr ) ; for ( b_kstr =
0 ; b_kstr < 6 ; b_kstr ++ ) { iobj_1 -> MotionSubspace -> data [ b_kstr ] =
msubspace_data [ b_kstr ] ; } b_kstr = iobj_1 -> PositionLimitsInternal ->
size [ 0 ] * iobj_1 -> PositionLimitsInternal -> size [ 1 ] ; iobj_1 ->
PositionLimitsInternal -> size [ 0 ] = 1 ; iobj_1 -> PositionLimitsInternal
-> size [ 1 ] = 2 ; miyl1rq4uj ( iobj_1 -> PositionLimitsInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 2 ; b_kstr ++ ) { iobj_1 ->
PositionLimitsInternal -> data [ b_kstr ] = poslim_data [ b_kstr ] ; } b_kstr
= iobj_1 -> HomePositionInternal -> size [ 0 ] ; iobj_1 ->
HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( iobj_1 ->
HomePositionInternal , b_kstr ) ; iobj_1 -> HomePositionInternal -> data [ 0
] = 0.0 ; obj -> JointInternal = iobj_1 ; obj -> Index = - 1.0 ; obj ->
ParentIndex = - 1.0 ; obj -> MassInternal = 1.0 ; obj -> CenterOfMassInternal
[ 0 ] = 0.0 ; obj -> CenterOfMassInternal [ 1 ] = 0.0 ; obj ->
CenterOfMassInternal [ 2 ] = 0.0 ; for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++
) { b_I [ b_kstr ] = 0 ; } b_I [ 0 ] = 1 ; b_I [ 4 ] = 1 ; b_I [ 8 ] = 1 ;
for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { obj -> InertiaInternal [ b_kstr
] = b_I [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ )
{ msubspace_data [ b_kstr + 6 * b_kstr ] = 1 ; } for ( b_kstr = 0 ; b_kstr <
36 ; b_kstr ++ ) { obj -> SpatialInertia [ b_kstr ] = msubspace_data [ b_kstr
] ; } obj -> CollisionsInternal = p1kacjwirim ( iobj_0 , 0.0 ) ; obj ->
matlabCodegenIsDeleted = false ; return b_obj ; } static alyuhn31fbfz *
et05vlhztn ( alyuhn31fbfz * obj , ogx4llc4arl * iobj_0 , ot4uovzkt0w * iobj_1
) { alyuhn31fbfz * b_obj ; e1n154mvbh * switch_expression ; real_T
poslim_data [ 12 ] ; int32_T b_kstr ; int32_T loop_ub ; char_T b_p [ 9 ] ;
char_T b [ 8 ] ; int8_T msubspace_data [ 36 ] ; int8_T b_I [ 9 ] ; int8_T tmp
[ 6 ] ; boolean_T b_bool ; static const char_T tmp_p [ 10 ] = { 'd' , 'u' ,
'm' , 'm' , 'y' , 'b' , 'o' , 'd' , 'y' , '8' } ; static const int8_T tmp_e [
16 ] = { 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 } ;
static const char_T tmp_i [ 14 ] = { 'd' , 'u' , 'm' , 'm' , 'y' , 'b' , 'o'
, 'd' , 'y' , '8' , '_' , 'j' , 'n' , 't' } ; static const char_T tmp_m [ 5 ]
= { 'f' , 'i' , 'x' , 'e' , 'd' } ; static const char_T tmp_g [ 8 ] = { 'r' ,
'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ; static const char_T tmp_j [ 9 ] =
{ 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' , 'i' , 'c' } ; int32_T exitg1 ;
b_obj = obj ; b_kstr = obj -> NameInternal -> size [ 0 ] * obj ->
NameInternal -> size [ 1 ] ; obj -> NameInternal -> size [ 0 ] = 1 ; obj ->
NameInternal -> size [ 1 ] = 10 ; e3n3mnwk4b ( obj -> NameInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 10 ; b_kstr ++ ) { obj -> NameInternal -> data
[ b_kstr ] = tmp_p [ b_kstr ] ; } iobj_1 -> InTree = false ; for ( b_kstr = 0
; b_kstr < 16 ; b_kstr ++ ) { iobj_1 -> JointToParentTransform [ b_kstr ] =
tmp_e [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { iobj_1 ->
ChildToJointTransform [ b_kstr ] = tmp_e [ b_kstr ] ; } b_kstr = iobj_1 ->
NameInternal -> size [ 0 ] * iobj_1 -> NameInternal -> size [ 1 ] ; iobj_1 ->
NameInternal -> size [ 0 ] = 1 ; iobj_1 -> NameInternal -> size [ 1 ] = 14 ;
e3n3mnwk4b ( iobj_1 -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr <
14 ; b_kstr ++ ) { iobj_1 -> NameInternal -> data [ b_kstr ] = tmp_i [ b_kstr
] ; } b_kstr = iobj_1 -> Type -> size [ 0 ] * iobj_1 -> Type -> size [ 1 ] ;
iobj_1 -> Type -> size [ 0 ] = 1 ; iobj_1 -> Type -> size [ 1 ] = 5 ;
e3n3mnwk4b ( iobj_1 -> Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ;
b_kstr ++ ) { iobj_1 -> Type -> data [ b_kstr ] = tmp_m [ b_kstr ] ; }
m2n1niwoq4 ( & switch_expression , 2 ) ; b_kstr = switch_expression -> size [
0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = iobj_1 -> Type -> size [ 1 ] ; e3n3mnwk4b (
switch_expression , b_kstr ) ; loop_ub = iobj_1 -> Type -> size [ 1 ] ; for (
b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [
b_kstr ] = iobj_1 -> Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr <
8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_g [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_j [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ]
= 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } poslim_data [ 0 ] = - 3.1415926535897931 ; poslim_data [ 1 ] =
3.1415926535897931 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; case 1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] =
0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++
) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } poslim_data [ 0 ] = - 0.5
; poslim_data [ 1 ] = 0.5 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } poslim_data [ 0 ] = 0.0 ; poslim_data [ 1 ]
= 0.0 ; iobj_1 -> VelocityNumber = 0.0 ; iobj_1 -> PositionNumber = 0.0 ;
iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 1 ] =
0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = iobj_1 ->
MotionSubspace -> size [ 0 ] * iobj_1 -> MotionSubspace -> size [ 1 ] ;
iobj_1 -> MotionSubspace -> size [ 0 ] = 6 ; iobj_1 -> MotionSubspace -> size
[ 1 ] = 1 ; miyl1rq4uj ( iobj_1 -> MotionSubspace , b_kstr ) ; for ( b_kstr =
0 ; b_kstr < 6 ; b_kstr ++ ) { iobj_1 -> MotionSubspace -> data [ b_kstr ] =
msubspace_data [ b_kstr ] ; } b_kstr = iobj_1 -> PositionLimitsInternal ->
size [ 0 ] * iobj_1 -> PositionLimitsInternal -> size [ 1 ] ; iobj_1 ->
PositionLimitsInternal -> size [ 0 ] = 1 ; iobj_1 -> PositionLimitsInternal
-> size [ 1 ] = 2 ; miyl1rq4uj ( iobj_1 -> PositionLimitsInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 2 ; b_kstr ++ ) { iobj_1 ->
PositionLimitsInternal -> data [ b_kstr ] = poslim_data [ b_kstr ] ; } b_kstr
= iobj_1 -> HomePositionInternal -> size [ 0 ] ; iobj_1 ->
HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( iobj_1 ->
HomePositionInternal , b_kstr ) ; iobj_1 -> HomePositionInternal -> data [ 0
] = 0.0 ; obj -> JointInternal = iobj_1 ; obj -> Index = - 1.0 ; obj ->
ParentIndex = - 1.0 ; obj -> MassInternal = 1.0 ; obj -> CenterOfMassInternal
[ 0 ] = 0.0 ; obj -> CenterOfMassInternal [ 1 ] = 0.0 ; obj ->
CenterOfMassInternal [ 2 ] = 0.0 ; for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++
) { b_I [ b_kstr ] = 0 ; } b_I [ 0 ] = 1 ; b_I [ 4 ] = 1 ; b_I [ 8 ] = 1 ;
for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { obj -> InertiaInternal [ b_kstr
] = b_I [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ )
{ msubspace_data [ b_kstr + 6 * b_kstr ] = 1 ; } for ( b_kstr = 0 ; b_kstr <
36 ; b_kstr ++ ) { obj -> SpatialInertia [ b_kstr ] = msubspace_data [ b_kstr
] ; } obj -> CollisionsInternal = p1kacjwirim ( iobj_0 , 0.0 ) ; obj ->
matlabCodegenIsDeleted = false ; return b_obj ; } static alyuhn31fbfz *
oct3agbiik ( alyuhn31fbfz * obj , ogx4llc4arl * iobj_0 , ot4uovzkt0w * iobj_1
) { alyuhn31fbfz * b_obj ; e1n154mvbh * switch_expression ; real_T
poslim_data [ 12 ] ; int32_T b_kstr ; int32_T loop_ub ; char_T b_p [ 9 ] ;
char_T b [ 8 ] ; int8_T msubspace_data [ 36 ] ; int8_T b_I [ 9 ] ; int8_T tmp
[ 6 ] ; boolean_T b_bool ; static const char_T tmp_p [ 10 ] = { 'd' , 'u' ,
'm' , 'm' , 'y' , 'b' , 'o' , 'd' , 'y' , '9' } ; static const int8_T tmp_e [
16 ] = { 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 } ;
static const char_T tmp_i [ 14 ] = { 'd' , 'u' , 'm' , 'm' , 'y' , 'b' , 'o'
, 'd' , 'y' , '9' , '_' , 'j' , 'n' , 't' } ; static const char_T tmp_m [ 5 ]
= { 'f' , 'i' , 'x' , 'e' , 'd' } ; static const char_T tmp_g [ 8 ] = { 'r' ,
'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ; static const char_T tmp_j [ 9 ] =
{ 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' , 'i' , 'c' } ; int32_T exitg1 ;
b_obj = obj ; b_kstr = obj -> NameInternal -> size [ 0 ] * obj ->
NameInternal -> size [ 1 ] ; obj -> NameInternal -> size [ 0 ] = 1 ; obj ->
NameInternal -> size [ 1 ] = 10 ; e3n3mnwk4b ( obj -> NameInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 10 ; b_kstr ++ ) { obj -> NameInternal -> data
[ b_kstr ] = tmp_p [ b_kstr ] ; } iobj_1 -> InTree = false ; for ( b_kstr = 0
; b_kstr < 16 ; b_kstr ++ ) { iobj_1 -> JointToParentTransform [ b_kstr ] =
tmp_e [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { iobj_1 ->
ChildToJointTransform [ b_kstr ] = tmp_e [ b_kstr ] ; } b_kstr = iobj_1 ->
NameInternal -> size [ 0 ] * iobj_1 -> NameInternal -> size [ 1 ] ; iobj_1 ->
NameInternal -> size [ 0 ] = 1 ; iobj_1 -> NameInternal -> size [ 1 ] = 14 ;
e3n3mnwk4b ( iobj_1 -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr <
14 ; b_kstr ++ ) { iobj_1 -> NameInternal -> data [ b_kstr ] = tmp_i [ b_kstr
] ; } b_kstr = iobj_1 -> Type -> size [ 0 ] * iobj_1 -> Type -> size [ 1 ] ;
iobj_1 -> Type -> size [ 0 ] = 1 ; iobj_1 -> Type -> size [ 1 ] = 5 ;
e3n3mnwk4b ( iobj_1 -> Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ;
b_kstr ++ ) { iobj_1 -> Type -> data [ b_kstr ] = tmp_m [ b_kstr ] ; }
m2n1niwoq4 ( & switch_expression , 2 ) ; b_kstr = switch_expression -> size [
0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = iobj_1 -> Type -> size [ 1 ] ; e3n3mnwk4b (
switch_expression , b_kstr ) ; loop_ub = iobj_1 -> Type -> size [ 1 ] ; for (
b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [
b_kstr ] = iobj_1 -> Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr <
8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_g [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_j [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ]
= 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } poslim_data [ 0 ] = - 3.1415926535897931 ; poslim_data [ 1 ] =
3.1415926535897931 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; case 1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] =
0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++
) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } poslim_data [ 0 ] = - 0.5
; poslim_data [ 1 ] = 0.5 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } poslim_data [ 0 ] = 0.0 ; poslim_data [ 1 ]
= 0.0 ; iobj_1 -> VelocityNumber = 0.0 ; iobj_1 -> PositionNumber = 0.0 ;
iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 1 ] =
0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = iobj_1 ->
MotionSubspace -> size [ 0 ] * iobj_1 -> MotionSubspace -> size [ 1 ] ;
iobj_1 -> MotionSubspace -> size [ 0 ] = 6 ; iobj_1 -> MotionSubspace -> size
[ 1 ] = 1 ; miyl1rq4uj ( iobj_1 -> MotionSubspace , b_kstr ) ; for ( b_kstr =
0 ; b_kstr < 6 ; b_kstr ++ ) { iobj_1 -> MotionSubspace -> data [ b_kstr ] =
msubspace_data [ b_kstr ] ; } b_kstr = iobj_1 -> PositionLimitsInternal ->
size [ 0 ] * iobj_1 -> PositionLimitsInternal -> size [ 1 ] ; iobj_1 ->
PositionLimitsInternal -> size [ 0 ] = 1 ; iobj_1 -> PositionLimitsInternal
-> size [ 1 ] = 2 ; miyl1rq4uj ( iobj_1 -> PositionLimitsInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 2 ; b_kstr ++ ) { iobj_1 ->
PositionLimitsInternal -> data [ b_kstr ] = poslim_data [ b_kstr ] ; } b_kstr
= iobj_1 -> HomePositionInternal -> size [ 0 ] ; iobj_1 ->
HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( iobj_1 ->
HomePositionInternal , b_kstr ) ; iobj_1 -> HomePositionInternal -> data [ 0
] = 0.0 ; obj -> JointInternal = iobj_1 ; obj -> Index = - 1.0 ; obj ->
ParentIndex = - 1.0 ; obj -> MassInternal = 1.0 ; obj -> CenterOfMassInternal
[ 0 ] = 0.0 ; obj -> CenterOfMassInternal [ 1 ] = 0.0 ; obj ->
CenterOfMassInternal [ 2 ] = 0.0 ; for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++
) { b_I [ b_kstr ] = 0 ; } b_I [ 0 ] = 1 ; b_I [ 4 ] = 1 ; b_I [ 8 ] = 1 ;
for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { obj -> InertiaInternal [ b_kstr
] = b_I [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ )
{ msubspace_data [ b_kstr + 6 * b_kstr ] = 1 ; } for ( b_kstr = 0 ; b_kstr <
36 ; b_kstr ++ ) { obj -> SpatialInertia [ b_kstr ] = msubspace_data [ b_kstr
] ; } obj -> CollisionsInternal = p1kacjwirim ( iobj_0 , 0.0 ) ; obj ->
matlabCodegenIsDeleted = false ; return b_obj ; } static alyuhn31fbfz *
jphupobpz2 ( alyuhn31fbfz * obj , ogx4llc4arl * iobj_0 , ot4uovzkt0w * iobj_1
) { alyuhn31fbfz * b_obj ; e1n154mvbh * switch_expression ; real_T
poslim_data [ 12 ] ; int32_T b_kstr ; int32_T loop_ub ; char_T b_p [ 9 ] ;
char_T b [ 8 ] ; int8_T msubspace_data [ 36 ] ; int8_T tmp [ 6 ] ; boolean_T
b_bool ; static const char_T tmp_p [ 5 ] = { 'l' , 'i' , 'n' , 'k' , '3' } ;
static const real_T tmp_e [ 9 ] = { 0.0017632141516686539 , -
6.368433412722551E-7 , - 0.00018371649145547766 , - 6.368433412722551E-7 ,
0.0017902634754422204 , - 6.8885291018321461E-6 , - 0.00018371649145547766 ,
- 6.8885291018321461E-6 , 7.50846247383926E-5 } ; static const real_T tmp_i [
36 ] = { 0.0017632141516686539 , - 6.368433412722551E-7 , -
0.00018371649145547766 , 0.0 , - 0.0140866554520649 , 5.22780188213871E-5 , -
6.368433412722551E-7 , 0.0017902634754422204 , - 6.8885291018321461E-6 ,
0.0140866554520649 , 0.0 , - 0.00142780695846381 , - 0.00018371649145547766 ,
- 6.8885291018321461E-6 , 7.50846247383926E-5 , - 5.22780188213871E-5 ,
0.00142780695846381 , 0.0 , 0.0 , 0.0140866554520649 , - 5.22780188213871E-5
, 0.13850917 , 0.0 , 0.0 , - 0.0140866554520649 , 0.0 , 0.00142780695846381 ,
0.0 , 0.13850917 , 0.0 , 5.22780188213871E-5 , - 0.00142780695846381 , 0.0 ,
0.0 , 0.0 , 0.13850917 } ; static const int8_T tmp_m [ 16 ] = { 1 , 0 , 0 , 0
, 0 , 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 } ; static const char_T tmp_g
[ 6 ] = { 'j' , 'o' , 'i' , 'n' , 't' , '2' } ; static const char_T tmp_j [ 8
] = { 'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ; static const char_T
tmp_f [ 9 ] = { 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' , 'i' , 'c' } ;
static const real_T tmp_c [ 16 ] = { 1.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 1.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.058 , 1.0 } ; static const
real_T tmp_k [ 16 ] = { 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 } ; static const real_T tmp_b [ 36 ]
= { 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 } ; int32_T
exitg1 ; b_obj = obj ; b_kstr = obj -> NameInternal -> size [ 0 ] * obj ->
NameInternal -> size [ 1 ] ; obj -> NameInternal -> size [ 0 ] = 1 ; obj ->
NameInternal -> size [ 1 ] = 5 ; e3n3mnwk4b ( obj -> NameInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { obj -> NameInternal -> data [
b_kstr ] = tmp_p [ b_kstr ] ; } obj -> ParentIndex = 2.0 ; obj ->
MassInternal = 0.13850917 ; obj -> CenterOfMassInternal [ 0 ] = 0.010308393 ;
obj -> CenterOfMassInternal [ 1 ] = 0.00037743363 ; obj ->
CenterOfMassInternal [ 2 ] = 0.10170197 ; for ( b_kstr = 0 ; b_kstr < 9 ;
b_kstr ++ ) { obj -> InertiaInternal [ b_kstr ] = tmp_e [ b_kstr ] ; } for (
b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) { obj -> SpatialInertia [ b_kstr ] =
tmp_i [ b_kstr ] ; } iobj_1 -> InTree = false ; for ( b_kstr = 0 ; b_kstr <
16 ; b_kstr ++ ) { iobj_1 -> JointToParentTransform [ b_kstr ] = tmp_m [
b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { iobj_1 ->
ChildToJointTransform [ b_kstr ] = tmp_m [ b_kstr ] ; } b_kstr = iobj_1 ->
NameInternal -> size [ 0 ] * iobj_1 -> NameInternal -> size [ 1 ] ; iobj_1 ->
NameInternal -> size [ 0 ] = 1 ; iobj_1 -> NameInternal -> size [ 1 ] = 6 ;
e3n3mnwk4b ( iobj_1 -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr <
6 ; b_kstr ++ ) { iobj_1 -> NameInternal -> data [ b_kstr ] = tmp_g [ b_kstr
] ; } b_kstr = iobj_1 -> Type -> size [ 0 ] * iobj_1 -> Type -> size [ 1 ] ;
iobj_1 -> Type -> size [ 0 ] = 1 ; iobj_1 -> Type -> size [ 1 ] = 8 ;
e3n3mnwk4b ( iobj_1 -> Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 8 ;
b_kstr ++ ) { iobj_1 -> Type -> data [ b_kstr ] = tmp_j [ b_kstr ] ; }
m2n1niwoq4 ( & switch_expression , 2 ) ; b_kstr = switch_expression -> size [
0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = iobj_1 -> Type -> size [ 1 ] ; e3n3mnwk4b (
switch_expression , b_kstr ) ; loop_ub = iobj_1 -> Type -> size [ 1 ] ; for (
b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [
b_kstr ] = iobj_1 -> Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr <
8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_j [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_f [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ]
= 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } poslim_data [ 0 ] = - 3.1415926535897931 ; poslim_data [ 1 ] =
3.1415926535897931 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; case 1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] =
0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++
) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } poslim_data [ 0 ] = - 0.5
; poslim_data [ 1 ] = 0.5 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } poslim_data [ 0 ] = 0.0 ; poslim_data [ 1 ]
= 0.0 ; iobj_1 -> VelocityNumber = 0.0 ; iobj_1 -> PositionNumber = 0.0 ;
iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 1 ] =
0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = iobj_1 ->
MotionSubspace -> size [ 0 ] * iobj_1 -> MotionSubspace -> size [ 1 ] ;
iobj_1 -> MotionSubspace -> size [ 0 ] = 6 ; iobj_1 -> MotionSubspace -> size
[ 1 ] = 1 ; miyl1rq4uj ( iobj_1 -> MotionSubspace , b_kstr ) ; for ( b_kstr =
0 ; b_kstr < 6 ; b_kstr ++ ) { iobj_1 -> MotionSubspace -> data [ b_kstr ] =
msubspace_data [ b_kstr ] ; } b_kstr = iobj_1 -> PositionLimitsInternal ->
size [ 0 ] * iobj_1 -> PositionLimitsInternal -> size [ 1 ] ; iobj_1 ->
PositionLimitsInternal -> size [ 0 ] = 1 ; iobj_1 -> PositionLimitsInternal
-> size [ 1 ] = 2 ; miyl1rq4uj ( iobj_1 -> PositionLimitsInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 2 ; b_kstr ++ ) { iobj_1 ->
PositionLimitsInternal -> data [ b_kstr ] = poslim_data [ b_kstr ] ; } b_kstr
= iobj_1 -> HomePositionInternal -> size [ 0 ] ; iobj_1 ->
HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( iobj_1 ->
HomePositionInternal , b_kstr ) ; iobj_1 -> HomePositionInternal -> data [ 0
] = 0.0 ; obj -> JointInternal = iobj_1 ; for ( b_kstr = 0 ; b_kstr < 16 ;
b_kstr ++ ) { obj -> JointInternal -> JointToParentTransform [ b_kstr ] =
tmp_c [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj ->
JointInternal -> ChildToJointTransform [ b_kstr ] = tmp_k [ b_kstr ] ; }
b_kstr = obj -> JointInternal -> MotionSubspace -> size [ 0 ] * obj ->
JointInternal -> MotionSubspace -> size [ 1 ] ; obj -> JointInternal ->
MotionSubspace -> size [ 0 ] = 6 ; obj -> JointInternal -> MotionSubspace ->
size [ 1 ] = 1 ; miyl1rq4uj ( obj -> JointInternal -> MotionSubspace , b_kstr
) ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { obj -> JointInternal ->
MotionSubspace -> data [ b_kstr ] = tmp_b [ b_kstr ] ; } obj -> JointInternal
-> InTree = true ; b_kstr = obj -> JointInternal -> PositionLimitsInternal ->
size [ 0 ] * obj -> JointInternal -> PositionLimitsInternal -> size [ 1 ] ;
obj -> JointInternal -> PositionLimitsInternal -> size [ 0 ] = 1 ; obj ->
JointInternal -> PositionLimitsInternal -> size [ 1 ] = 2 ; miyl1rq4uj ( obj
-> JointInternal -> PositionLimitsInternal , b_kstr ) ; obj -> JointInternal
-> PositionLimitsInternal -> data [ 0 ] = - 1.7907 ; obj -> JointInternal ->
PositionLimitsInternal -> data [ obj -> JointInternal ->
PositionLimitsInternal -> size [ 0 ] ] = 1.5708 ; obj -> JointInternal ->
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal -> JointAxisInternal [ 1
] = 1.0 ; obj -> JointInternal -> JointAxisInternal [ 2 ] = 0.0 ; b_kstr =
obj -> JointInternal -> HomePositionInternal -> size [ 0 ] ; obj ->
JointInternal -> HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( obj ->
JointInternal -> HomePositionInternal , b_kstr ) ; obj -> JointInternal ->
HomePositionInternal -> data [ 0 ] = 0.0 ; obj -> CollisionsInternal =
p1kacjwirim ( iobj_0 , 0.0 ) ; obj -> matlabCodegenIsDeleted = false ; return
b_obj ; } static alyuhn31fbfz * bl5mnhfgqx ( alyuhn31fbfz * obj , ogx4llc4arl
* iobj_0 , ot4uovzkt0w * iobj_1 ) { alyuhn31fbfz * b_obj ; e1n154mvbh *
switch_expression ; real_T poslim_data [ 12 ] ; int32_T b_kstr ; int32_T
loop_ub ; char_T b_p [ 9 ] ; char_T b [ 8 ] ; int8_T msubspace_data [ 36 ] ;
int8_T tmp [ 6 ] ; boolean_T b_bool ; static const char_T tmp_p [ 5 ] = { 'l'
, 'i' , 'n' , 'k' , '4' } ; static const real_T tmp_e [ 9 ] = {
3.0680964548160883E-5 , - 5.97440725674762E-6 , - 2.973545034809753E-6 , -
5.97440725674762E-6 , 0.001339392874088295 , - 2.3100409454553815E-11 , -
2.973545034809753E-6 , - 2.3100409454553815E-11 , 0.0013486539735492638 } ;
static const real_T tmp_i [ 36 ] = { 3.0680964548160883E-5 , -
5.97440725674762E-6 , - 2.973545034809753E-6 , 0.0 , - 2.9752646170879803E-5
, 5.16776256140592E-5 , - 5.97440725674762E-6 , 0.001339392874088295 , -
2.3100409454553815E-11 , 2.9752646170879803E-5 , 0.0 , - 0.0120678498884958 ,
- 2.973545034809753E-6 , - 2.3100409454553815E-11 , 0.0013486539735492638 , -
5.16776256140592E-5 , 0.0120678498884958 , 0.0 , 0.0 , 2.9752646170879803E-5
, - 5.16776256140592E-5 , 0.13274562 , 0.0 , 0.0 , - 2.9752646170879803E-5 ,
0.0 , 0.0120678498884958 , 0.0 , 0.13274562 , 0.0 , 5.16776256140592E-5 , -
0.0120678498884958 , 0.0 , 0.0 , 0.0 , 0.13274562 } ; static const int8_T
tmp_m [ 16 ] = { 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 ,
1 } ; static const char_T tmp_g [ 6 ] = { 'j' , 'o' , 'i' , 'n' , 't' , '3' }
; static const char_T tmp_j [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't'
, 'e' } ; static const char_T tmp_f [ 9 ] = { 'p' , 'r' , 'i' , 's' , 'm' ,
'a' , 't' , 'i' , 'c' } ; static const real_T tmp_c [ 16 ] = { 1.0 , 0.0 , -
0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.024 , 0.0 ,
0.128 , 1.0 } ; static const real_T tmp_k [ 16 ] = { 1.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 } ;
static const real_T tmp_b [ 36 ] = { 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0
, 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0
, 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0
, 0.0 , 0.0 , 0.0 } ; int32_T exitg1 ; b_obj = obj ; b_kstr = obj ->
NameInternal -> size [ 0 ] * obj -> NameInternal -> size [ 1 ] ; obj ->
NameInternal -> size [ 0 ] = 1 ; obj -> NameInternal -> size [ 1 ] = 5 ;
e3n3mnwk4b ( obj -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ;
b_kstr ++ ) { obj -> NameInternal -> data [ b_kstr ] = tmp_p [ b_kstr ] ; }
obj -> ParentIndex = 3.0 ; obj -> MassInternal = 0.13274562 ; obj ->
CenterOfMassInternal [ 0 ] = 0.09090959 ; obj -> CenterOfMassInternal [ 1 ] =
0.00038929816 ; obj -> CenterOfMassInternal [ 2 ] = 0.00022413279 ; for (
b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { obj -> InertiaInternal [ b_kstr ] =
tmp_e [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) { obj ->
SpatialInertia [ b_kstr ] = tmp_i [ b_kstr ] ; } iobj_1 -> InTree = false ;
for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { iobj_1 ->
JointToParentTransform [ b_kstr ] = tmp_m [ b_kstr ] ; } for ( b_kstr = 0 ;
b_kstr < 16 ; b_kstr ++ ) { iobj_1 -> ChildToJointTransform [ b_kstr ] =
tmp_m [ b_kstr ] ; } b_kstr = iobj_1 -> NameInternal -> size [ 0 ] * iobj_1
-> NameInternal -> size [ 1 ] ; iobj_1 -> NameInternal -> size [ 0 ] = 1 ;
iobj_1 -> NameInternal -> size [ 1 ] = 6 ; e3n3mnwk4b ( iobj_1 ->
NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
iobj_1 -> NameInternal -> data [ b_kstr ] = tmp_g [ b_kstr ] ; } b_kstr =
iobj_1 -> Type -> size [ 0 ] * iobj_1 -> Type -> size [ 1 ] ; iobj_1 -> Type
-> size [ 0 ] = 1 ; iobj_1 -> Type -> size [ 1 ] = 8 ; e3n3mnwk4b ( iobj_1 ->
Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { iobj_1 ->
Type -> data [ b_kstr ] = tmp_j [ b_kstr ] ; } m2n1niwoq4 ( &
switch_expression , 2 ) ; b_kstr = switch_expression -> size [ 0 ] *
switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = iobj_1 -> Type -> size [ 1 ] ; e3n3mnwk4b (
switch_expression , b_kstr ) ; loop_ub = iobj_1 -> Type -> size [ 1 ] ; for (
b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [
b_kstr ] = iobj_1 -> Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr <
8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_j [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_f [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ]
= 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } poslim_data [ 0 ] = - 3.1415926535897931 ; poslim_data [ 1 ] =
3.1415926535897931 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; case 1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] =
0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++
) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } poslim_data [ 0 ] = - 0.5
; poslim_data [ 1 ] = 0.5 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } poslim_data [ 0 ] = 0.0 ; poslim_data [ 1 ]
= 0.0 ; iobj_1 -> VelocityNumber = 0.0 ; iobj_1 -> PositionNumber = 0.0 ;
iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 1 ] =
0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = iobj_1 ->
MotionSubspace -> size [ 0 ] * iobj_1 -> MotionSubspace -> size [ 1 ] ;
iobj_1 -> MotionSubspace -> size [ 0 ] = 6 ; iobj_1 -> MotionSubspace -> size
[ 1 ] = 1 ; miyl1rq4uj ( iobj_1 -> MotionSubspace , b_kstr ) ; for ( b_kstr =
0 ; b_kstr < 6 ; b_kstr ++ ) { iobj_1 -> MotionSubspace -> data [ b_kstr ] =
msubspace_data [ b_kstr ] ; } b_kstr = iobj_1 -> PositionLimitsInternal ->
size [ 0 ] * iobj_1 -> PositionLimitsInternal -> size [ 1 ] ; iobj_1 ->
PositionLimitsInternal -> size [ 0 ] = 1 ; iobj_1 -> PositionLimitsInternal
-> size [ 1 ] = 2 ; miyl1rq4uj ( iobj_1 -> PositionLimitsInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 2 ; b_kstr ++ ) { iobj_1 ->
PositionLimitsInternal -> data [ b_kstr ] = poslim_data [ b_kstr ] ; } b_kstr
= iobj_1 -> HomePositionInternal -> size [ 0 ] ; iobj_1 ->
HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( iobj_1 ->
HomePositionInternal , b_kstr ) ; iobj_1 -> HomePositionInternal -> data [ 0
] = 0.0 ; obj -> JointInternal = iobj_1 ; for ( b_kstr = 0 ; b_kstr < 16 ;
b_kstr ++ ) { obj -> JointInternal -> JointToParentTransform [ b_kstr ] =
tmp_c [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj ->
JointInternal -> ChildToJointTransform [ b_kstr ] = tmp_k [ b_kstr ] ; }
b_kstr = obj -> JointInternal -> MotionSubspace -> size [ 0 ] * obj ->
JointInternal -> MotionSubspace -> size [ 1 ] ; obj -> JointInternal ->
MotionSubspace -> size [ 0 ] = 6 ; obj -> JointInternal -> MotionSubspace ->
size [ 1 ] = 1 ; miyl1rq4uj ( obj -> JointInternal -> MotionSubspace , b_kstr
) ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { obj -> JointInternal ->
MotionSubspace -> data [ b_kstr ] = tmp_b [ b_kstr ] ; } obj -> JointInternal
-> InTree = true ; b_kstr = obj -> JointInternal -> PositionLimitsInternal ->
size [ 0 ] * obj -> JointInternal -> PositionLimitsInternal -> size [ 1 ] ;
obj -> JointInternal -> PositionLimitsInternal -> size [ 0 ] = 1 ; obj ->
JointInternal -> PositionLimitsInternal -> size [ 1 ] = 2 ; miyl1rq4uj ( obj
-> JointInternal -> PositionLimitsInternal , b_kstr ) ; obj -> JointInternal
-> PositionLimitsInternal -> data [ 0 ] = - 0.94248 ; obj -> JointInternal ->
PositionLimitsInternal -> data [ obj -> JointInternal ->
PositionLimitsInternal -> size [ 0 ] ] = 1.3823 ; obj -> JointInternal ->
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal -> JointAxisInternal [ 1
] = 1.0 ; obj -> JointInternal -> JointAxisInternal [ 2 ] = 0.0 ; b_kstr =
obj -> JointInternal -> HomePositionInternal -> size [ 0 ] ; obj ->
JointInternal -> HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( obj ->
JointInternal -> HomePositionInternal , b_kstr ) ; obj -> JointInternal ->
HomePositionInternal -> data [ 0 ] = 0.0 ; obj -> CollisionsInternal =
p1kacjwirim ( iobj_0 , 0.0 ) ; obj -> matlabCodegenIsDeleted = false ; return
b_obj ; } static alyuhn31fbfz * gwskexfjch ( alyuhn31fbfz * obj , ogx4llc4arl
* iobj_0 , ot4uovzkt0w * iobj_1 ) { alyuhn31fbfz * b_obj ; e1n154mvbh *
switch_expression ; real_T poslim_data [ 12 ] ; int32_T b_kstr ; int32_T
loop_ub ; char_T b_p [ 9 ] ; char_T b [ 8 ] ; int8_T msubspace_data [ 36 ] ;
int8_T tmp [ 6 ] ; boolean_T b_bool ; static const char_T tmp_p [ 5 ] = { 'l'
, 'i' , 'n' , 'k' , '5' } ; static const real_T tmp_e [ 9 ] = {
9.2255918030988742E-5 , - 2.3333544264028212E-9 , - 5.747628606408824E-5 , -
2.3333544264028212E-9 , 0.00036736039366238232 , - 4.705171964849182E-10 , -
5.747628606408824E-5 , - 4.705171964849182E-10 , 0.00037312211067028395 } ;
static const real_T tmp_i [ 36 ] = { 9.2255918030988742E-5 , -
2.3333544264028212E-9 , - 5.747628606408824E-5 , 0.0 , -
0.0012771916071217678 , 5.2782757440640491E-8 , - 2.3333544264028212E-9 ,
0.00036736039366238232 , - 4.705171964849182E-10 , 0.0012771916071217678 ,
0.0 , - 0.00633375509355615 , - 5.747628606408824E-5 , -
4.705171964849182E-10 , 0.00037312211067028395 , - 5.2782757440640491E-8 ,
0.00633375509355615 , 0.0 , 0.0 , 0.0012771916071217678 , -
5.2782757440640491E-8 , 0.14327573 , 0.0 , 0.0 , - 0.0012771916071217678 ,
0.0 , 0.00633375509355615 , 0.0 , 0.14327573 , 0.0 , 5.2782757440640491E-8 ,
- 0.00633375509355615 , 0.0 , 0.0 , 0.0 , 0.14327573 } ; static const int8_T
tmp_m [ 16 ] = { 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 ,
1 } ; static const char_T tmp_g [ 6 ] = { 'j' , 'o' , 'i' , 'n' , 't' , '4' }
; static const char_T tmp_j [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't'
, 'e' } ; static const char_T tmp_f [ 9 ] = { 'p' , 'r' , 'i' , 's' , 'm' ,
'a' , 't' , 'i' , 'c' } ; static const real_T tmp_c [ 16 ] = { 1.0 , 0.0 , -
0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.124 , 0.0 , 0.0
, 1.0 } ; static const real_T tmp_k [ 16 ] = { 1.0 , 0.0 , 0.0 , 0.0 , 0.0 ,
1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 } ; static
const real_T tmp_b [ 36 ] = { 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 0.0 } ; int32_T exitg1 ; b_obj = obj ; b_kstr = obj -> NameInternal ->
size [ 0 ] * obj -> NameInternal -> size [ 1 ] ; obj -> NameInternal -> size
[ 0 ] = 1 ; obj -> NameInternal -> size [ 1 ] = 5 ; e3n3mnwk4b ( obj ->
NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { obj
-> NameInternal -> data [ b_kstr ] = tmp_p [ b_kstr ] ; } obj -> ParentIndex
= 4.0 ; obj -> MassInternal = 0.14327573 ; obj -> CenterOfMassInternal [ 0 ]
= 0.044206755 ; obj -> CenterOfMassInternal [ 1 ] = 3.6839985E-7 ; obj ->
CenterOfMassInternal [ 2 ] = 0.0089142216 ; for ( b_kstr = 0 ; b_kstr < 9 ;
b_kstr ++ ) { obj -> InertiaInternal [ b_kstr ] = tmp_e [ b_kstr ] ; } for (
b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) { obj -> SpatialInertia [ b_kstr ] =
tmp_i [ b_kstr ] ; } iobj_1 -> InTree = false ; for ( b_kstr = 0 ; b_kstr <
16 ; b_kstr ++ ) { iobj_1 -> JointToParentTransform [ b_kstr ] = tmp_m [
b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { iobj_1 ->
ChildToJointTransform [ b_kstr ] = tmp_m [ b_kstr ] ; } b_kstr = iobj_1 ->
NameInternal -> size [ 0 ] * iobj_1 -> NameInternal -> size [ 1 ] ; iobj_1 ->
NameInternal -> size [ 0 ] = 1 ; iobj_1 -> NameInternal -> size [ 1 ] = 6 ;
e3n3mnwk4b ( iobj_1 -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr <
6 ; b_kstr ++ ) { iobj_1 -> NameInternal -> data [ b_kstr ] = tmp_g [ b_kstr
] ; } b_kstr = iobj_1 -> Type -> size [ 0 ] * iobj_1 -> Type -> size [ 1 ] ;
iobj_1 -> Type -> size [ 0 ] = 1 ; iobj_1 -> Type -> size [ 1 ] = 8 ;
e3n3mnwk4b ( iobj_1 -> Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 8 ;
b_kstr ++ ) { iobj_1 -> Type -> data [ b_kstr ] = tmp_j [ b_kstr ] ; }
m2n1niwoq4 ( & switch_expression , 2 ) ; b_kstr = switch_expression -> size [
0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = iobj_1 -> Type -> size [ 1 ] ; e3n3mnwk4b (
switch_expression , b_kstr ) ; loop_ub = iobj_1 -> Type -> size [ 1 ] ; for (
b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [
b_kstr ] = iobj_1 -> Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr <
8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_j [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_f [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ]
= 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } poslim_data [ 0 ] = - 3.1415926535897931 ; poslim_data [ 1 ] =
3.1415926535897931 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; case 1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] =
0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++
) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } poslim_data [ 0 ] = - 0.5
; poslim_data [ 1 ] = 0.5 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } poslim_data [ 0 ] = 0.0 ; poslim_data [ 1 ]
= 0.0 ; iobj_1 -> VelocityNumber = 0.0 ; iobj_1 -> PositionNumber = 0.0 ;
iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 1 ] =
0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = iobj_1 ->
MotionSubspace -> size [ 0 ] * iobj_1 -> MotionSubspace -> size [ 1 ] ;
iobj_1 -> MotionSubspace -> size [ 0 ] = 6 ; iobj_1 -> MotionSubspace -> size
[ 1 ] = 1 ; miyl1rq4uj ( iobj_1 -> MotionSubspace , b_kstr ) ; for ( b_kstr =
0 ; b_kstr < 6 ; b_kstr ++ ) { iobj_1 -> MotionSubspace -> data [ b_kstr ] =
msubspace_data [ b_kstr ] ; } b_kstr = iobj_1 -> PositionLimitsInternal ->
size [ 0 ] * iobj_1 -> PositionLimitsInternal -> size [ 1 ] ; iobj_1 ->
PositionLimitsInternal -> size [ 0 ] = 1 ; iobj_1 -> PositionLimitsInternal
-> size [ 1 ] = 2 ; miyl1rq4uj ( iobj_1 -> PositionLimitsInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 2 ; b_kstr ++ ) { iobj_1 ->
PositionLimitsInternal -> data [ b_kstr ] = poslim_data [ b_kstr ] ; } b_kstr
= iobj_1 -> HomePositionInternal -> size [ 0 ] ; iobj_1 ->
HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( iobj_1 ->
HomePositionInternal , b_kstr ) ; iobj_1 -> HomePositionInternal -> data [ 0
] = 0.0 ; obj -> JointInternal = iobj_1 ; for ( b_kstr = 0 ; b_kstr < 16 ;
b_kstr ++ ) { obj -> JointInternal -> JointToParentTransform [ b_kstr ] =
tmp_c [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj ->
JointInternal -> ChildToJointTransform [ b_kstr ] = tmp_k [ b_kstr ] ; }
b_kstr = obj -> JointInternal -> MotionSubspace -> size [ 0 ] * obj ->
JointInternal -> MotionSubspace -> size [ 1 ] ; obj -> JointInternal ->
MotionSubspace -> size [ 0 ] = 6 ; obj -> JointInternal -> MotionSubspace ->
size [ 1 ] = 1 ; miyl1rq4uj ( obj -> JointInternal -> MotionSubspace , b_kstr
) ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { obj -> JointInternal ->
MotionSubspace -> data [ b_kstr ] = tmp_b [ b_kstr ] ; } obj -> JointInternal
-> InTree = true ; b_kstr = obj -> JointInternal -> PositionLimitsInternal ->
size [ 0 ] * obj -> JointInternal -> PositionLimitsInternal -> size [ 1 ] ;
obj -> JointInternal -> PositionLimitsInternal -> size [ 0 ] = 1 ; obj ->
JointInternal -> PositionLimitsInternal -> size [ 1 ] = 2 ; miyl1rq4uj ( obj
-> JointInternal -> PositionLimitsInternal , b_kstr ) ; obj -> JointInternal
-> PositionLimitsInternal -> data [ 0 ] = - 1.7907 ; obj -> JointInternal ->
PositionLimitsInternal -> data [ obj -> JointInternal ->
PositionLimitsInternal -> size [ 0 ] ] = 2.042 ; obj -> JointInternal ->
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal -> JointAxisInternal [ 1
] = 1.0 ; obj -> JointInternal -> JointAxisInternal [ 2 ] = 0.0 ; b_kstr =
obj -> JointInternal -> HomePositionInternal -> size [ 0 ] ; obj ->
JointInternal -> HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( obj ->
JointInternal -> HomePositionInternal , b_kstr ) ; obj -> JointInternal ->
HomePositionInternal -> data [ 0 ] = 0.0 ; obj -> CollisionsInternal =
p1kacjwirim ( iobj_0 , 0.0 ) ; obj -> matlabCodegenIsDeleted = false ; return
b_obj ; } static alyuhn31fbfz * nuiglfj4n2 ( alyuhn31fbfz * obj , ogx4llc4arl
* iobj_0 , ot4uovzkt0w * iobj_1 ) { alyuhn31fbfz * b_obj ; e1n154mvbh *
switch_expression ; real_T poslim_data [ 12 ] ; int32_T b_kstr ; int32_T
loop_ub ; char_T b_p [ 9 ] ; char_T b [ 8 ] ; int8_T msubspace_data [ 36 ] ;
int8_T tmp [ 6 ] ; boolean_T b_bool ; static const char_T tmp_p [ 17 ] = {
'e' , 'n' , 'd' , '_' , 'e' , 'f' , 'f' , 'e' , 'c' , 't' , 'o' , 'r' , '_' ,
'l' , 'i' , 'n' , 'k' } ; static const real_T tmp_e [ 9 ] = { 1.0E-6 , 0.0 ,
0.0 , 0.0 , 1.0E-6 , 0.0 , 0.0 , 0.0 , 1.0E-6 } ; static const real_T tmp_i [
36 ] = { 1.0E-6 , 0.0 , 0.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 1.0E-6 , 0.0 , 0.0 ,
0.0 , - 0.0 , 0.0 , 0.0 , 1.0E-6 , - 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , - 0.0 ,
0.001 , 0.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 0.0 , 0.001 , 0.0 , 0.0 , - 0.0 , 0.0
, 0.0 , 0.0 , 0.001 } ; static const int8_T tmp_m [ 16 ] = { 1 , 0 , 0 , 0 ,
0 , 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 } ; static const char_T tmp_g [
18 ] = { 'e' , 'n' , 'd' , '_' , 'e' , 'f' , 'f' , 'e' , 'c' , 't' , 'o' ,
'r' , '_' , 'j' , 'o' , 'i' , 'n' , 't' } ; static const char_T tmp_j [ 5 ] =
{ 'f' , 'i' , 'x' , 'e' , 'd' } ; static const char_T tmp_f [ 8 ] = { 'r' ,
'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ; static const char_T tmp_c [ 9 ] =
{ 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' , 'i' , 'c' } ; static const real_T
tmp_k [ 16 ] = { 1.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0
, 1.0 , 0.0 , 0.13 , 0.0 , 0.0 , 1.0 } ; static const real_T tmp_b [ 16 ] = {
1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 1.0 } ; int32_T exitg1 ; b_obj = obj ; b_kstr = obj ->
NameInternal -> size [ 0 ] * obj -> NameInternal -> size [ 1 ] ; obj ->
NameInternal -> size [ 0 ] = 1 ; obj -> NameInternal -> size [ 1 ] = 17 ;
e3n3mnwk4b ( obj -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 17
; b_kstr ++ ) { obj -> NameInternal -> data [ b_kstr ] = tmp_p [ b_kstr ] ; }
obj -> ParentIndex = 5.0 ; obj -> MassInternal = 0.001 ; obj ->
CenterOfMassInternal [ 0 ] = 0.0 ; obj -> CenterOfMassInternal [ 1 ] = 0.0 ;
obj -> CenterOfMassInternal [ 2 ] = 0.0 ; for ( b_kstr = 0 ; b_kstr < 9 ;
b_kstr ++ ) { obj -> InertiaInternal [ b_kstr ] = tmp_e [ b_kstr ] ; } for (
b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) { obj -> SpatialInertia [ b_kstr ] =
tmp_i [ b_kstr ] ; } iobj_1 -> InTree = false ; for ( b_kstr = 0 ; b_kstr <
16 ; b_kstr ++ ) { iobj_1 -> JointToParentTransform [ b_kstr ] = tmp_m [
b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { iobj_1 ->
ChildToJointTransform [ b_kstr ] = tmp_m [ b_kstr ] ; } b_kstr = iobj_1 ->
NameInternal -> size [ 0 ] * iobj_1 -> NameInternal -> size [ 1 ] ; iobj_1 ->
NameInternal -> size [ 0 ] = 1 ; iobj_1 -> NameInternal -> size [ 1 ] = 18 ;
e3n3mnwk4b ( iobj_1 -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr <
18 ; b_kstr ++ ) { iobj_1 -> NameInternal -> data [ b_kstr ] = tmp_g [ b_kstr
] ; } b_kstr = iobj_1 -> Type -> size [ 0 ] * iobj_1 -> Type -> size [ 1 ] ;
iobj_1 -> Type -> size [ 0 ] = 1 ; iobj_1 -> Type -> size [ 1 ] = 5 ;
e3n3mnwk4b ( iobj_1 -> Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ;
b_kstr ++ ) { iobj_1 -> Type -> data [ b_kstr ] = tmp_j [ b_kstr ] ; }
m2n1niwoq4 ( & switch_expression , 2 ) ; b_kstr = switch_expression -> size [
0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = iobj_1 -> Type -> size [ 1 ] ; e3n3mnwk4b (
switch_expression , b_kstr ) ; loop_ub = iobj_1 -> Type -> size [ 1 ] ; for (
b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [
b_kstr ] = iobj_1 -> Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr <
8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_f [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_c [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ]
= 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } poslim_data [ 0 ] = - 3.1415926535897931 ; poslim_data [ 1 ] =
3.1415926535897931 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; case 1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] =
0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++
) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } poslim_data [ 0 ] = - 0.5
; poslim_data [ 1 ] = 0.5 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } poslim_data [ 0 ] = 0.0 ; poslim_data [ 1 ]
= 0.0 ; iobj_1 -> VelocityNumber = 0.0 ; iobj_1 -> PositionNumber = 0.0 ;
iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 1 ] =
0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = iobj_1 ->
MotionSubspace -> size [ 0 ] * iobj_1 -> MotionSubspace -> size [ 1 ] ;
iobj_1 -> MotionSubspace -> size [ 0 ] = 6 ; iobj_1 -> MotionSubspace -> size
[ 1 ] = 1 ; miyl1rq4uj ( iobj_1 -> MotionSubspace , b_kstr ) ; for ( b_kstr =
0 ; b_kstr < 6 ; b_kstr ++ ) { iobj_1 -> MotionSubspace -> data [ b_kstr ] =
msubspace_data [ b_kstr ] ; } b_kstr = iobj_1 -> PositionLimitsInternal ->
size [ 0 ] * iobj_1 -> PositionLimitsInternal -> size [ 1 ] ; iobj_1 ->
PositionLimitsInternal -> size [ 0 ] = 1 ; iobj_1 -> PositionLimitsInternal
-> size [ 1 ] = 2 ; miyl1rq4uj ( iobj_1 -> PositionLimitsInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 2 ; b_kstr ++ ) { iobj_1 ->
PositionLimitsInternal -> data [ b_kstr ] = poslim_data [ b_kstr ] ; } b_kstr
= iobj_1 -> HomePositionInternal -> size [ 0 ] ; iobj_1 ->
HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( iobj_1 ->
HomePositionInternal , b_kstr ) ; iobj_1 -> HomePositionInternal -> data [ 0
] = 0.0 ; obj -> JointInternal = iobj_1 ; for ( b_kstr = 0 ; b_kstr < 16 ;
b_kstr ++ ) { obj -> JointInternal -> JointToParentTransform [ b_kstr ] =
tmp_k [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj ->
JointInternal -> ChildToJointTransform [ b_kstr ] = tmp_b [ b_kstr ] ; }
b_kstr = obj -> JointInternal -> MotionSubspace -> size [ 0 ] * obj ->
JointInternal -> MotionSubspace -> size [ 1 ] ; obj -> JointInternal ->
MotionSubspace -> size [ 0 ] = 6 ; obj -> JointInternal -> MotionSubspace ->
size [ 1 ] = 1 ; miyl1rq4uj ( obj -> JointInternal -> MotionSubspace , b_kstr
) ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { obj -> JointInternal ->
MotionSubspace -> data [ b_kstr ] = 0.0 ; } obj -> JointInternal -> InTree =
true ; b_kstr = obj -> JointInternal -> PositionLimitsInternal -> size [ 0 ]
* obj -> JointInternal -> PositionLimitsInternal -> size [ 1 ] ; obj ->
JointInternal -> PositionLimitsInternal -> size [ 0 ] = 1 ; obj ->
JointInternal -> PositionLimitsInternal -> size [ 1 ] = 2 ; miyl1rq4uj ( obj
-> JointInternal -> PositionLimitsInternal , b_kstr ) ; obj -> JointInternal
-> PositionLimitsInternal -> data [ 0 ] = 0.0 ; obj -> JointInternal ->
PositionLimitsInternal -> data [ obj -> JointInternal ->
PositionLimitsInternal -> size [ 0 ] ] = 0.0 ; obj -> JointInternal ->
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal -> JointAxisInternal [ 1
] = 0.0 ; obj -> JointInternal -> JointAxisInternal [ 2 ] = 0.0 ; b_kstr =
obj -> JointInternal -> HomePositionInternal -> size [ 0 ] ; obj ->
JointInternal -> HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( obj ->
JointInternal -> HomePositionInternal , b_kstr ) ; obj -> JointInternal ->
HomePositionInternal -> data [ 0 ] = 0.0 ; obj -> CollisionsInternal =
p1kacjwirim ( iobj_0 , 0.0 ) ; obj -> matlabCodegenIsDeleted = false ; return
b_obj ; } static alyuhn31fbfz * nxwlc1tqzg ( alyuhn31fbfz * obj , ogx4llc4arl
* iobj_0 , ot4uovzkt0w * iobj_1 ) { alyuhn31fbfz * b_obj ; e1n154mvbh *
switch_expression ; real_T poslim_data [ 12 ] ; int32_T b_kstr ; int32_T
loop_ub ; char_T b_p [ 9 ] ; char_T b [ 8 ] ; int8_T msubspace_data [ 36 ] ;
int8_T tmp [ 6 ] ; boolean_T b_bool ; static const char_T tmp_p [ 12 ] = {
'g' , 'r' , 'i' , 'p' , 'p' , 'e' , 'r' , '_' , 'l' , 'i' , 'n' , 'k' } ;
static const real_T tmp_e [ 9 ] = { 0.001 , 0.0 , 0.0 , 0.0 , 0.001 , 0.0 ,
0.0 , 0.0 , 0.001 } ; static const real_T tmp_i [ 36 ] = { 0.001 , 0.0 , 0.0
, 0.0 , - 0.0 , 0.0 , 0.0 , 0.001 , 0.0 , 0.0 , 0.0 , - 0.0 , 0.0 , 0.0 ,
0.001 , - 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , - 0.0 , 0.017 , 0.0 , 0.0 , - 0.0 ,
0.0 , 0.0 , 0.0 , 0.017 , 0.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 0.0 , 0.017 } ;
static const int8_T tmp_m [ 16 ] = { 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 ,
1 , 0 , 0 , 0 , 0 , 1 } ; static const char_T tmp_g [ 7 ] = { 'g' , 'r' , 'i'
, 'p' , 'p' , 'e' , 'r' } ; static const char_T tmp_j [ 9 ] = { 'p' , 'r' ,
'i' , 's' , 'm' , 'a' , 't' , 'i' , 'c' } ; static const char_T tmp_f [ 8 ] =
{ 'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ; static const real_T tmp_c
[ 16 ] = { 1.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0
, 0.0 , 0.0817 , 0.019 , 0.0 , 1.0 } ; static const real_T tmp_k [ 16 ] = {
1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 1.0 } ; static const real_T tmp_b [ 36 ] = { 0.0 , 0.0 , 0.0 ,
0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 } ; int32_T exitg1 ; b_obj = obj ;
b_kstr = obj -> NameInternal -> size [ 0 ] * obj -> NameInternal -> size [ 1
] ; obj -> NameInternal -> size [ 0 ] = 1 ; obj -> NameInternal -> size [ 1 ]
= 12 ; e3n3mnwk4b ( obj -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ;
b_kstr < 12 ; b_kstr ++ ) { obj -> NameInternal -> data [ b_kstr ] = tmp_p [
b_kstr ] ; } obj -> ParentIndex = 5.0 ; obj -> MassInternal = 0.017 ; obj ->
CenterOfMassInternal [ 0 ] = 0.0 ; obj -> CenterOfMassInternal [ 1 ] = 0.0 ;
obj -> CenterOfMassInternal [ 2 ] = 0.0 ; for ( b_kstr = 0 ; b_kstr < 9 ;
b_kstr ++ ) { obj -> InertiaInternal [ b_kstr ] = tmp_e [ b_kstr ] ; } for (
b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) { obj -> SpatialInertia [ b_kstr ] =
tmp_i [ b_kstr ] ; } iobj_1 -> InTree = false ; for ( b_kstr = 0 ; b_kstr <
16 ; b_kstr ++ ) { iobj_1 -> JointToParentTransform [ b_kstr ] = tmp_m [
b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { iobj_1 ->
ChildToJointTransform [ b_kstr ] = tmp_m [ b_kstr ] ; } b_kstr = iobj_1 ->
NameInternal -> size [ 0 ] * iobj_1 -> NameInternal -> size [ 1 ] ; iobj_1 ->
NameInternal -> size [ 0 ] = 1 ; iobj_1 -> NameInternal -> size [ 1 ] = 7 ;
e3n3mnwk4b ( iobj_1 -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr <
7 ; b_kstr ++ ) { iobj_1 -> NameInternal -> data [ b_kstr ] = tmp_g [ b_kstr
] ; } b_kstr = iobj_1 -> Type -> size [ 0 ] * iobj_1 -> Type -> size [ 1 ] ;
iobj_1 -> Type -> size [ 0 ] = 1 ; iobj_1 -> Type -> size [ 1 ] = 9 ;
e3n3mnwk4b ( iobj_1 -> Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 9 ;
b_kstr ++ ) { iobj_1 -> Type -> data [ b_kstr ] = tmp_j [ b_kstr ] ; }
m2n1niwoq4 ( & switch_expression , 2 ) ; b_kstr = switch_expression -> size [
0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = iobj_1 -> Type -> size [ 1 ] ; e3n3mnwk4b (
switch_expression , b_kstr ) ; loop_ub = iobj_1 -> Type -> size [ 1 ] ; for (
b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [
b_kstr ] = iobj_1 -> Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr <
8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_f [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_j [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ]
= 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } poslim_data [ 0 ] = - 3.1415926535897931 ; poslim_data [ 1 ] =
3.1415926535897931 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; case 1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] =
0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++
) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } poslim_data [ 0 ] = - 0.5
; poslim_data [ 1 ] = 0.5 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } poslim_data [ 0 ] = 0.0 ; poslim_data [ 1 ]
= 0.0 ; iobj_1 -> VelocityNumber = 0.0 ; iobj_1 -> PositionNumber = 0.0 ;
iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 1 ] =
0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = iobj_1 ->
MotionSubspace -> size [ 0 ] * iobj_1 -> MotionSubspace -> size [ 1 ] ;
iobj_1 -> MotionSubspace -> size [ 0 ] = 6 ; iobj_1 -> MotionSubspace -> size
[ 1 ] = 1 ; miyl1rq4uj ( iobj_1 -> MotionSubspace , b_kstr ) ; for ( b_kstr =
0 ; b_kstr < 6 ; b_kstr ++ ) { iobj_1 -> MotionSubspace -> data [ b_kstr ] =
msubspace_data [ b_kstr ] ; } b_kstr = iobj_1 -> PositionLimitsInternal ->
size [ 0 ] * iobj_1 -> PositionLimitsInternal -> size [ 1 ] ; iobj_1 ->
PositionLimitsInternal -> size [ 0 ] = 1 ; iobj_1 -> PositionLimitsInternal
-> size [ 1 ] = 2 ; miyl1rq4uj ( iobj_1 -> PositionLimitsInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 2 ; b_kstr ++ ) { iobj_1 ->
PositionLimitsInternal -> data [ b_kstr ] = poslim_data [ b_kstr ] ; } b_kstr
= iobj_1 -> HomePositionInternal -> size [ 0 ] ; iobj_1 ->
HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( iobj_1 ->
HomePositionInternal , b_kstr ) ; iobj_1 -> HomePositionInternal -> data [ 0
] = 0.0 ; obj -> JointInternal = iobj_1 ; for ( b_kstr = 0 ; b_kstr < 16 ;
b_kstr ++ ) { obj -> JointInternal -> JointToParentTransform [ b_kstr ] =
tmp_c [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj ->
JointInternal -> ChildToJointTransform [ b_kstr ] = tmp_k [ b_kstr ] ; }
b_kstr = obj -> JointInternal -> MotionSubspace -> size [ 0 ] * obj ->
JointInternal -> MotionSubspace -> size [ 1 ] ; obj -> JointInternal ->
MotionSubspace -> size [ 0 ] = 6 ; obj -> JointInternal -> MotionSubspace ->
size [ 1 ] = 1 ; miyl1rq4uj ( obj -> JointInternal -> MotionSubspace , b_kstr
) ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { obj -> JointInternal ->
MotionSubspace -> data [ b_kstr ] = tmp_b [ b_kstr ] ; } obj -> JointInternal
-> InTree = true ; b_kstr = obj -> JointInternal -> PositionLimitsInternal ->
size [ 0 ] * obj -> JointInternal -> PositionLimitsInternal -> size [ 1 ] ;
obj -> JointInternal -> PositionLimitsInternal -> size [ 0 ] = 1 ; obj ->
JointInternal -> PositionLimitsInternal -> size [ 1 ] = 2 ; miyl1rq4uj ( obj
-> JointInternal -> PositionLimitsInternal , b_kstr ) ; obj -> JointInternal
-> PositionLimitsInternal -> data [ 0 ] = - 0.01 ; obj -> JointInternal ->
PositionLimitsInternal -> data [ obj -> JointInternal ->
PositionLimitsInternal -> size [ 0 ] ] = 0.019 ; obj -> JointInternal ->
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal -> JointAxisInternal [ 1
] = 1.0 ; obj -> JointInternal -> JointAxisInternal [ 2 ] = 0.0 ; b_kstr =
obj -> JointInternal -> HomePositionInternal -> size [ 0 ] ; obj ->
JointInternal -> HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( obj ->
JointInternal -> HomePositionInternal , b_kstr ) ; obj -> JointInternal ->
HomePositionInternal -> data [ 0 ] = 0.0 ; obj -> CollisionsInternal =
p1kacjwirim ( iobj_0 , 0.0 ) ; obj -> matlabCodegenIsDeleted = false ; return
b_obj ; } static alyuhn31fbfz * gtyzt122qi ( alyuhn31fbfz * obj , ogx4llc4arl
* iobj_0 , ot4uovzkt0w * iobj_1 ) { alyuhn31fbfz * b_obj ; e1n154mvbh *
switch_expression ; real_T poslim_data [ 12 ] ; int32_T b_kstr ; int32_T
loop_ub ; char_T b_p [ 9 ] ; char_T b [ 8 ] ; int8_T msubspace_data [ 36 ] ;
int8_T tmp [ 6 ] ; boolean_T b_bool ; static const char_T tmp_p [ 16 ] = {
'g' , 'r' , 'i' , 'p' , 'p' , 'e' , 'r' , '_' , 'l' , 'i' , 'n' , 'k' , '_' ,
's' , 'u' , 'b' } ; static const real_T tmp_e [ 9 ] = { 0.001 , 0.0 , 0.0 ,
0.0 , 0.001 , 0.0 , 0.0 , 0.0 , 0.001 } ; static const real_T tmp_i [ 36 ] =
{ 0.001 , 0.0 , 0.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 0.001 , 0.0 , 0.0 , 0.0 , -
0.0 , 0.0 , 0.0 , 0.001 , - 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , - 0.0 , 0.017 , 0.0
, 0.0 , - 0.0 , 0.0 , 0.0 , 0.0 , 0.017 , 0.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 0.0
, 0.017 } ; static const int8_T tmp_m [ 16 ] = { 1 , 0 , 0 , 0 , 0 , 1 , 0 ,
0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 } ; static const char_T tmp_g [ 11 ] = {
'g' , 'r' , 'i' , 'p' , 'p' , 'e' , 'r' , '_' , 's' , 'u' , 'b' } ; static
const char_T tmp_j [ 9 ] = { 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' , 'i' ,
'c' } ; static const char_T tmp_f [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l' , 'u'
, 't' , 'e' } ; static const real_T tmp_c [ 16 ] = { 1.0 , 0.0 , - 0.0 , 0.0
, 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0817 , - 0.019 , 0.0 ,
1.0 } ; static const real_T tmp_k [ 16 ] = { 1.0 , 0.0 , 0.0 , 0.0 , 0.0 ,
1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 } ; static
const real_T tmp_b [ 36 ] = { 0.0 , 0.0 , 0.0 , 0.0 , - 1.0 , 0.0 , 0.0 , 0.0
, 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0
, 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0
, 0.0 , 0.0 } ; int32_T exitg1 ; b_obj = obj ; b_kstr = obj -> NameInternal
-> size [ 0 ] * obj -> NameInternal -> size [ 1 ] ; obj -> NameInternal ->
size [ 0 ] = 1 ; obj -> NameInternal -> size [ 1 ] = 16 ; e3n3mnwk4b ( obj ->
NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj
-> NameInternal -> data [ b_kstr ] = tmp_p [ b_kstr ] ; } obj -> ParentIndex
= 5.0 ; obj -> MassInternal = 0.017 ; obj -> CenterOfMassInternal [ 0 ] = 0.0
; obj -> CenterOfMassInternal [ 1 ] = 0.0 ; obj -> CenterOfMassInternal [ 2 ]
= 0.0 ; for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { obj -> InertiaInternal
[ b_kstr ] = tmp_e [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++
) { obj -> SpatialInertia [ b_kstr ] = tmp_i [ b_kstr ] ; } iobj_1 -> InTree
= false ; for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { iobj_1 ->
JointToParentTransform [ b_kstr ] = tmp_m [ b_kstr ] ; } for ( b_kstr = 0 ;
b_kstr < 16 ; b_kstr ++ ) { iobj_1 -> ChildToJointTransform [ b_kstr ] =
tmp_m [ b_kstr ] ; } b_kstr = iobj_1 -> NameInternal -> size [ 0 ] * iobj_1
-> NameInternal -> size [ 1 ] ; iobj_1 -> NameInternal -> size [ 0 ] = 1 ;
iobj_1 -> NameInternal -> size [ 1 ] = 11 ; e3n3mnwk4b ( iobj_1 ->
NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 11 ; b_kstr ++ ) {
iobj_1 -> NameInternal -> data [ b_kstr ] = tmp_g [ b_kstr ] ; } b_kstr =
iobj_1 -> Type -> size [ 0 ] * iobj_1 -> Type -> size [ 1 ] ; iobj_1 -> Type
-> size [ 0 ] = 1 ; iobj_1 -> Type -> size [ 1 ] = 9 ; e3n3mnwk4b ( iobj_1 ->
Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { iobj_1 ->
Type -> data [ b_kstr ] = tmp_j [ b_kstr ] ; } m2n1niwoq4 ( &
switch_expression , 2 ) ; b_kstr = switch_expression -> size [ 0 ] *
switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = iobj_1 -> Type -> size [ 1 ] ; e3n3mnwk4b (
switch_expression , b_kstr ) ; loop_ub = iobj_1 -> Type -> size [ 1 ] ; for (
b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [
b_kstr ] = iobj_1 -> Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr <
8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_f [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_j [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ]
= 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } poslim_data [ 0 ] = - 3.1415926535897931 ; poslim_data [ 1 ] =
3.1415926535897931 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; case 1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] =
0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++
) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } poslim_data [ 0 ] = - 0.5
; poslim_data [ 1 ] = 0.5 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } poslim_data [ 0 ] = 0.0 ; poslim_data [ 1 ]
= 0.0 ; iobj_1 -> VelocityNumber = 0.0 ; iobj_1 -> PositionNumber = 0.0 ;
iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 1 ] =
0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = iobj_1 ->
MotionSubspace -> size [ 0 ] * iobj_1 -> MotionSubspace -> size [ 1 ] ;
iobj_1 -> MotionSubspace -> size [ 0 ] = 6 ; iobj_1 -> MotionSubspace -> size
[ 1 ] = 1 ; miyl1rq4uj ( iobj_1 -> MotionSubspace , b_kstr ) ; for ( b_kstr =
0 ; b_kstr < 6 ; b_kstr ++ ) { iobj_1 -> MotionSubspace -> data [ b_kstr ] =
msubspace_data [ b_kstr ] ; } b_kstr = iobj_1 -> PositionLimitsInternal ->
size [ 0 ] * iobj_1 -> PositionLimitsInternal -> size [ 1 ] ; iobj_1 ->
PositionLimitsInternal -> size [ 0 ] = 1 ; iobj_1 -> PositionLimitsInternal
-> size [ 1 ] = 2 ; miyl1rq4uj ( iobj_1 -> PositionLimitsInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 2 ; b_kstr ++ ) { iobj_1 ->
PositionLimitsInternal -> data [ b_kstr ] = poslim_data [ b_kstr ] ; } b_kstr
= iobj_1 -> HomePositionInternal -> size [ 0 ] ; iobj_1 ->
HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( iobj_1 ->
HomePositionInternal , b_kstr ) ; iobj_1 -> HomePositionInternal -> data [ 0
] = 0.0 ; obj -> JointInternal = iobj_1 ; for ( b_kstr = 0 ; b_kstr < 16 ;
b_kstr ++ ) { obj -> JointInternal -> JointToParentTransform [ b_kstr ] =
tmp_c [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj ->
JointInternal -> ChildToJointTransform [ b_kstr ] = tmp_k [ b_kstr ] ; }
b_kstr = obj -> JointInternal -> MotionSubspace -> size [ 0 ] * obj ->
JointInternal -> MotionSubspace -> size [ 1 ] ; obj -> JointInternal ->
MotionSubspace -> size [ 0 ] = 6 ; obj -> JointInternal -> MotionSubspace ->
size [ 1 ] = 1 ; miyl1rq4uj ( obj -> JointInternal -> MotionSubspace , b_kstr
) ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { obj -> JointInternal ->
MotionSubspace -> data [ b_kstr ] = tmp_b [ b_kstr ] ; } obj -> JointInternal
-> InTree = true ; b_kstr = obj -> JointInternal -> PositionLimitsInternal ->
size [ 0 ] * obj -> JointInternal -> PositionLimitsInternal -> size [ 1 ] ;
obj -> JointInternal -> PositionLimitsInternal -> size [ 0 ] = 1 ; obj ->
JointInternal -> PositionLimitsInternal -> size [ 1 ] = 2 ; miyl1rq4uj ( obj
-> JointInternal -> PositionLimitsInternal , b_kstr ) ; obj -> JointInternal
-> PositionLimitsInternal -> data [ 0 ] = - 0.01 ; obj -> JointInternal ->
PositionLimitsInternal -> data [ obj -> JointInternal ->
PositionLimitsInternal -> size [ 0 ] ] = 0.019 ; obj -> JointInternal ->
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal -> JointAxisInternal [ 1
] = - 1.0 ; obj -> JointInternal -> JointAxisInternal [ 2 ] = 0.0 ; b_kstr =
obj -> JointInternal -> HomePositionInternal -> size [ 0 ] ; obj ->
JointInternal -> HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( obj ->
JointInternal -> HomePositionInternal , b_kstr ) ; obj -> JointInternal ->
HomePositionInternal -> data [ 0 ] = 0.0 ; obj -> CollisionsInternal =
p1kacjwirim ( iobj_0 , 0.0 ) ; obj -> matlabCodegenIsDeleted = false ; return
b_obj ; } static alyuhn31fbfz * dyhbwd1qgw ( alyuhn31fbfz * obj , ogx4llc4arl
* iobj_0 , ot4uovzkt0w * iobj_1 ) { alyuhn31fbfz * b_obj ; e1n154mvbh *
switch_expression ; real_T poslim_data [ 12 ] ; int32_T b_kstr ; int32_T
loop_ub ; char_T b_p [ 9 ] ; char_T b [ 8 ] ; int8_T msubspace_data [ 36 ] ;
int8_T tmp [ 6 ] ; boolean_T b_bool ; static const char_T tmp_p [ 12 ] = {
'e' , 'n' , 'd' , '_' , 'e' , 'f' , 'f' , 'e' , 'c' , 't' , 'o' , 'r' } ;
static const real_T tmp_e [ 36 ] = { 0.0 , 0.0 , 0.0 , 0.0 , - 0.0 , 0.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 0.0 , - 0.0 , 0.0 , 0.0 ,
0.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 ,
0.0 , - 0.0 , 0.0 , 0.0 , 0.0 , 0.0 } ; static const int8_T tmp_i [ 16 ] = {
1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 } ; static
const char_T tmp_m [ 16 ] = { 'e' , 'n' , 'd' , '_' , 'e' , 'f' , 'f' , 'e' ,
'c' , 't' , 'o' , 'r' , '_' , 'j' , 'n' , 't' } ; static const char_T tmp_g [
5 ] = { 'f' , 'i' , 'x' , 'e' , 'd' } ; static const char_T tmp_j [ 8 ] = {
'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ; static const char_T tmp_f [
9 ] = { 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' , 'i' , 'c' } ; static const
real_T tmp_c [ 16 ] = { 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 1.0 , 0.0 , 0.12 , 0.0 , 0.0 , 1.0 } ; static const real_T tmp_k [ 16 ]
= { 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 ,
0.0 , 0.0 , 0.0 , 1.0 } ; int32_T exitg1 ; b_obj = obj ; b_kstr = obj ->
NameInternal -> size [ 0 ] * obj -> NameInternal -> size [ 1 ] ; obj ->
NameInternal -> size [ 0 ] = 1 ; obj -> NameInternal -> size [ 1 ] = 12 ;
e3n3mnwk4b ( obj -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 12
; b_kstr ++ ) { obj -> NameInternal -> data [ b_kstr ] = tmp_p [ b_kstr ] ; }
obj -> ParentIndex = 5.0 ; obj -> MassInternal = 0.0 ; obj ->
CenterOfMassInternal [ 0 ] = 0.0 ; obj -> CenterOfMassInternal [ 1 ] = 0.0 ;
obj -> CenterOfMassInternal [ 2 ] = 0.0 ; for ( b_kstr = 0 ; b_kstr < 9 ;
b_kstr ++ ) { obj -> InertiaInternal [ b_kstr ] = 0.0 ; } for ( b_kstr = 0 ;
b_kstr < 36 ; b_kstr ++ ) { obj -> SpatialInertia [ b_kstr ] = tmp_e [ b_kstr
] ; } iobj_1 -> InTree = false ; for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ )
{ iobj_1 -> JointToParentTransform [ b_kstr ] = tmp_i [ b_kstr ] ; } for (
b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { iobj_1 -> ChildToJointTransform [
b_kstr ] = tmp_i [ b_kstr ] ; } b_kstr = iobj_1 -> NameInternal -> size [ 0 ]
* iobj_1 -> NameInternal -> size [ 1 ] ; iobj_1 -> NameInternal -> size [ 0 ]
= 1 ; iobj_1 -> NameInternal -> size [ 1 ] = 16 ; e3n3mnwk4b ( iobj_1 ->
NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) {
iobj_1 -> NameInternal -> data [ b_kstr ] = tmp_m [ b_kstr ] ; } b_kstr =
iobj_1 -> Type -> size [ 0 ] * iobj_1 -> Type -> size [ 1 ] ; iobj_1 -> Type
-> size [ 0 ] = 1 ; iobj_1 -> Type -> size [ 1 ] = 5 ; e3n3mnwk4b ( iobj_1 ->
Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { iobj_1 ->
Type -> data [ b_kstr ] = tmp_g [ b_kstr ] ; } m2n1niwoq4 ( &
switch_expression , 2 ) ; b_kstr = switch_expression -> size [ 0 ] *
switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = iobj_1 -> Type -> size [ 1 ] ; e3n3mnwk4b (
switch_expression , b_kstr ) ; loop_ub = iobj_1 -> Type -> size [ 1 ] ; for (
b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [
b_kstr ] = iobj_1 -> Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr <
8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_j [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_f [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ]
= 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } poslim_data [ 0 ] = - 3.1415926535897931 ; poslim_data [ 1 ] =
3.1415926535897931 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; case 1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] =
0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++
) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } poslim_data [ 0 ] = - 0.5
; poslim_data [ 1 ] = 0.5 ; iobj_1 -> VelocityNumber = 1.0 ; iobj_1 ->
PositionNumber = 1.0 ; iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 ->
JointAxisInternal [ 1 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 1.0 ;
break ; default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } poslim_data [ 0 ] = 0.0 ; poslim_data [ 1 ]
= 0.0 ; iobj_1 -> VelocityNumber = 0.0 ; iobj_1 -> PositionNumber = 0.0 ;
iobj_1 -> JointAxisInternal [ 0 ] = 0.0 ; iobj_1 -> JointAxisInternal [ 1 ] =
0.0 ; iobj_1 -> JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = iobj_1 ->
MotionSubspace -> size [ 0 ] * iobj_1 -> MotionSubspace -> size [ 1 ] ;
iobj_1 -> MotionSubspace -> size [ 0 ] = 6 ; iobj_1 -> MotionSubspace -> size
[ 1 ] = 1 ; miyl1rq4uj ( iobj_1 -> MotionSubspace , b_kstr ) ; for ( b_kstr =
0 ; b_kstr < 6 ; b_kstr ++ ) { iobj_1 -> MotionSubspace -> data [ b_kstr ] =
msubspace_data [ b_kstr ] ; } b_kstr = iobj_1 -> PositionLimitsInternal ->
size [ 0 ] * iobj_1 -> PositionLimitsInternal -> size [ 1 ] ; iobj_1 ->
PositionLimitsInternal -> size [ 0 ] = 1 ; iobj_1 -> PositionLimitsInternal
-> size [ 1 ] = 2 ; miyl1rq4uj ( iobj_1 -> PositionLimitsInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 2 ; b_kstr ++ ) { iobj_1 ->
PositionLimitsInternal -> data [ b_kstr ] = poslim_data [ b_kstr ] ; } b_kstr
= iobj_1 -> HomePositionInternal -> size [ 0 ] ; iobj_1 ->
HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( iobj_1 ->
HomePositionInternal , b_kstr ) ; iobj_1 -> HomePositionInternal -> data [ 0
] = 0.0 ; obj -> JointInternal = iobj_1 ; for ( b_kstr = 0 ; b_kstr < 16 ;
b_kstr ++ ) { obj -> JointInternal -> JointToParentTransform [ b_kstr ] =
tmp_c [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj ->
JointInternal -> ChildToJointTransform [ b_kstr ] = tmp_k [ b_kstr ] ; }
b_kstr = obj -> JointInternal -> MotionSubspace -> size [ 0 ] * obj ->
JointInternal -> MotionSubspace -> size [ 1 ] ; obj -> JointInternal ->
MotionSubspace -> size [ 0 ] = 6 ; obj -> JointInternal -> MotionSubspace ->
size [ 1 ] = 1 ; miyl1rq4uj ( obj -> JointInternal -> MotionSubspace , b_kstr
) ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { obj -> JointInternal ->
MotionSubspace -> data [ b_kstr ] = 0.0 ; } obj -> JointInternal -> InTree =
true ; b_kstr = obj -> JointInternal -> PositionLimitsInternal -> size [ 0 ]
* obj -> JointInternal -> PositionLimitsInternal -> size [ 1 ] ; obj ->
JointInternal -> PositionLimitsInternal -> size [ 0 ] = 1 ; obj ->
JointInternal -> PositionLimitsInternal -> size [ 1 ] = 2 ; miyl1rq4uj ( obj
-> JointInternal -> PositionLimitsInternal , b_kstr ) ; obj -> JointInternal
-> PositionLimitsInternal -> data [ 0 ] = 0.0 ; obj -> JointInternal ->
PositionLimitsInternal -> data [ obj -> JointInternal ->
PositionLimitsInternal -> size [ 0 ] ] = 0.0 ; obj -> JointInternal ->
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal -> JointAxisInternal [ 1
] = 0.0 ; obj -> JointInternal -> JointAxisInternal [ 2 ] = 0.0 ; b_kstr =
obj -> JointInternal -> HomePositionInternal -> size [ 0 ] ; obj ->
JointInternal -> HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( obj ->
JointInternal -> HomePositionInternal , b_kstr ) ; obj -> JointInternal ->
HomePositionInternal -> data [ 0 ] = 0.0 ; obj -> CollisionsInternal =
p1kacjwirim ( iobj_0 , 0.0 ) ; obj -> matlabCodegenIsDeleted = false ; return
b_obj ; } static ot4uovzkt0w * g1japxta3w ( ot4uovzkt0w * obj , const
e1n154mvbh * jname ) { e1n154mvbh * switch_expression ; ot4uovzkt0w * b_obj ;
real_T poslim_data [ 12 ] ; int32_T b_kstr ; int32_T loop_ub ; char_T b_p [ 9
] ; char_T b [ 8 ] ; int8_T msubspace_data [ 36 ] ; int8_T tmp [ 6 ] ;
boolean_T b_bool ; static const int8_T tmp_p [ 16 ] = { 1 , 0 , 0 , 0 , 0 , 1
, 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 } ; static const char_T tmp_e [ 5 ] =
{ 'f' , 'i' , 'x' , 'e' , 'd' } ; static const char_T tmp_i [ 8 ] = { 'r' ,
'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ; static const char_T tmp_m [ 9 ] =
{ 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' , 'i' , 'c' } ; int32_T exitg1 ;
obj -> InTree = false ; for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj ->
JointToParentTransform [ b_kstr ] = tmp_p [ b_kstr ] ; } for ( b_kstr = 0 ;
b_kstr < 16 ; b_kstr ++ ) { obj -> ChildToJointTransform [ b_kstr ] = tmp_p [
b_kstr ] ; } b_obj = obj ; b_kstr = obj -> NameInternal -> size [ 0 ] * obj
-> NameInternal -> size [ 1 ] ; obj -> NameInternal -> size [ 0 ] = 1 ; obj
-> NameInternal -> size [ 1 ] = jname -> size [ 1 ] ; e3n3mnwk4b ( obj ->
NameInternal , b_kstr ) ; loop_ub = jname -> size [ 1 ] - 1 ; for ( b_kstr =
0 ; b_kstr <= loop_ub ; b_kstr ++ ) { obj -> NameInternal -> data [ b_kstr ]
= jname -> data [ b_kstr ] ; } b_kstr = obj -> Type -> size [ 0 ] * obj ->
Type -> size [ 1 ] ; obj -> Type -> size [ 0 ] = 1 ; obj -> Type -> size [ 1
] = 5 ; e3n3mnwk4b ( obj -> Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ;
b_kstr ++ ) { obj -> Type -> data [ b_kstr ] = tmp_e [ b_kstr ] ; }
m2n1niwoq4 ( & switch_expression , 2 ) ; b_kstr = switch_expression -> size [
0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = obj -> Type -> size [ 1 ] ; e3n3mnwk4b (
switch_expression , b_kstr ) ; loop_ub = obj -> Type -> size [ 1 ] ; for (
b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [
b_kstr ] = obj -> Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 8 ;
b_kstr ++ ) { b [ b_kstr ] = tmp_i [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_m [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ]
= 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } poslim_data [ 0 ] = - 3.1415926535897931 ; poslim_data [ 1 ] =
3.1415926535897931 ; obj -> VelocityNumber = 1.0 ; obj -> PositionNumber =
1.0 ; obj -> JointAxisInternal [ 0 ] = 0.0 ; obj -> JointAxisInternal [ 1 ] =
0.0 ; obj -> JointAxisInternal [ 2 ] = 1.0 ; break ; case 1 : tmp [ 0 ] = 0 ;
tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 1
; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] =
tmp [ b_kstr ] ; } poslim_data [ 0 ] = - 0.5 ; poslim_data [ 1 ] = 0.5 ; obj
-> VelocityNumber = 1.0 ; obj -> PositionNumber = 1.0 ; obj ->
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointAxisInternal [ 1 ] = 0.0 ; obj ->
JointAxisInternal [ 2 ] = 1.0 ; break ; default : for ( b_kstr = 0 ; b_kstr <
6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = 0 ; } poslim_data [ 0 ] = 0.0 ;
poslim_data [ 1 ] = 0.0 ; obj -> VelocityNumber = 0.0 ; obj -> PositionNumber
= 0.0 ; obj -> JointAxisInternal [ 0 ] = 0.0 ; obj -> JointAxisInternal [ 1 ]
= 0.0 ; obj -> JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = obj ->
MotionSubspace -> size [ 0 ] * obj -> MotionSubspace -> size [ 1 ] ; obj ->
MotionSubspace -> size [ 0 ] = 6 ; obj -> MotionSubspace -> size [ 1 ] = 1 ;
miyl1rq4uj ( obj -> MotionSubspace , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 6
; b_kstr ++ ) { obj -> MotionSubspace -> data [ b_kstr ] = msubspace_data [
b_kstr ] ; } b_kstr = obj -> PositionLimitsInternal -> size [ 0 ] * obj ->
PositionLimitsInternal -> size [ 1 ] ; obj -> PositionLimitsInternal -> size
[ 0 ] = 1 ; obj -> PositionLimitsInternal -> size [ 1 ] = 2 ; miyl1rq4uj (
obj -> PositionLimitsInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 2 ;
b_kstr ++ ) { obj -> PositionLimitsInternal -> data [ b_kstr ] = poslim_data
[ b_kstr ] ; } b_kstr = obj -> HomePositionInternal -> size [ 0 ] ; obj ->
HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( obj ->
HomePositionInternal , b_kstr ) ; obj -> HomePositionInternal -> data [ 0 ] =
0.0 ; return b_obj ; } static mjv2tzlyyigm * jgbwvk33lx ( mjv2tzlyyigm * obj
) { alyuhn31fbfz * iobj_2 ; e1n154mvbh * jname ; mjv2tzlyyigm * b_obj ;
ogx4llc4arl * iobj_0 ; ot4uovzkt0w * iobj_1 ; real_T poslim_data [ 12 ] ;
int32_T b_kstr ; int32_T loop_ub ; char_T b_p [ 9 ] ; char_T b [ 8 ] ; int8_T
msubspace_data [ 36 ] ; int8_T tmp [ 6 ] ; boolean_T b_bool ; static const
char_T tmp_p [ 5 ] = { 'l' , 'i' , 'n' , 'k' , '1' } ; static const real_T
tmp_e [ 9 ] = { 1.2506407081630646E-5 , 0.0 , - 1.755774678296497E-7 , 0.0 ,
2.1907079879448666E-5 , 0.0 , - 1.755774678296497E-7 , 0.0 ,
1.9274903797818021E-5 } ; static const real_T tmp_i [ 36 ] = {
1.2506407081630646E-5 , 0.0 , - 1.755774678296497E-7 , 0.0 ,
9.63401131614482E-6 , 0.0 , 0.0 , 2.1907079879448666E-5 , 0.0 , -
9.63401131614482E-6 , 0.0 , - 2.4429201311861481E-5 , - 1.755774678296497E-7
, 0.0 , 1.9274903797818021E-5 , - 0.0 , 2.4429201311861481E-5 , 0.0 , 0.0 , -
9.63401131614482E-6 , - 0.0 , 0.079119962 , 0.0 , 0.0 , 9.63401131614482E-6 ,
0.0 , 2.4429201311861481E-5 , 0.0 , 0.079119962 , 0.0 , 0.0 , -
2.4429201311861481E-5 , 0.0 , 0.0 , 0.0 , 0.079119962 } ; static const int8_T
tmp_m [ 16 ] = { 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 ,
1 } ; static const char_T tmp_g [ 11 ] = { 'w' , 'o' , 'r' , 'l' , 'd' , '_'
, 'f' , 'i' , 'x' , 'e' , 'd' } ; static const char_T tmp_j [ 5 ] = { 'f' ,
'i' , 'x' , 'e' , 'd' } ; static const char_T tmp_f [ 8 ] = { 'r' , 'e' , 'v'
, 'o' , 'l' , 'u' , 't' , 'e' } ; static const char_T tmp_c [ 9 ] = { 'p' ,
'r' , 'i' , 's' , 'm' , 'a' , 't' , 'i' , 'c' } ; static const real_T tmp_k [
16 ] = { 1.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 1.0 } ; static const real_T tmp_b [ 16 ] = { 1.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 1.0 } ; static const char_T tmp_n [ 5 ] = { 'l' , 'i' , 'n' , 'k' , '2'
} ; static const real_T tmp_l [ 9 ] = { 0.00025597667209157054 ,
2.2027827360445393E-11 , 1.0251971350492796E-6 , 2.2027827360445393E-11 ,
0.00025410280331852224 , - 2.4941021265155473E-6 , 1.0251971350492796E-6 , -
2.4941021265155473E-6 , 1.8888027986020151E-5 } ; static const real_T tmp_d [
36 ] = { 0.00025597667209157054 , 2.2027827360445393E-11 ,
1.0251971350492796E-6 , 0.0 , - 0.004667731499421 , 5.3182680022675086E-5 ,
2.2027827360445393E-11 , 0.00025410280331852224 , - 2.4941021265155473E-6 ,
0.004667731499421 , 0.0 , 2.9703975819561897E-5 , 1.0251971350492796E-6 , -
2.4941021265155473E-6 , 1.8888027986020151E-5 , - 5.3182680022675086E-5 , -
2.9703975819561897E-5 , 0.0 , 0.0 , 0.004667731499421 , -
5.3182680022675086E-5 , 0.098406837 , 0.0 , 0.0 , - 0.004667731499421 , 0.0 ,
- 2.9703975819561897E-5 , 0.0 , 0.098406837 , 0.0 , 5.3182680022675086E-5 ,
2.9703975819561897E-5 , 0.0 , 0.0 , 0.0 , 0.098406837 } ; static const char_T
tmp_o [ 6 ] = { 'j' , 'o' , 'i' , 'n' , 't' , '1' } ; static const real_T
tmp_dz [ 16 ] = { 1.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0
, 1.0 , 0.0 , 0.012 , 0.0 , 0.017 , 1.0 } ; static const real_T tmp_fs [ 36 ]
= { 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 } ; static
const char_T tmp_ck [ 5 ] = { 'w' , 'o' , 'r' , 'l' , 'd' } ; int32_T exitg1
; b_obj = obj ; obj -> NumBodies = 9.0 ; iobj_0 = & obj -> _pobj0 [ 0 ] ;
iobj_1 = & obj -> _pobj1 [ 0 ] ; iobj_2 = & obj -> _pobj2 [ 0 ] ; obj ->
Bodies [ 0 ] = lwkmqybjwb ( & ( & obj -> _pobj2 [ 0 ] ) [ 0 ] , & iobj_0 [ 0
] , & iobj_1 [ 0 ] ) ; obj -> Bodies [ 1 ] = e0kx4oauic ( & iobj_2 [ 1 ] , &
iobj_0 [ 1 ] , & iobj_1 [ 1 ] ) ; obj -> Bodies [ 2 ] = mpzyhljwzq ( & iobj_2
[ 2 ] , & iobj_0 [ 2 ] , & iobj_1 [ 2 ] ) ; obj -> Bodies [ 3 ] = axww4prr0o
( & iobj_2 [ 3 ] , & iobj_0 [ 3 ] , & iobj_1 [ 3 ] ) ; obj -> Bodies [ 4 ] =
oh2xpcdbml ( & iobj_2 [ 4 ] , & iobj_0 [ 4 ] , & iobj_1 [ 4 ] ) ; obj ->
Bodies [ 5 ] = hcmkep3fjc ( & iobj_2 [ 5 ] , & iobj_0 [ 5 ] , & iobj_1 [ 5 ]
) ; obj -> Bodies [ 6 ] = eearo4zkrq ( & iobj_2 [ 6 ] , & iobj_0 [ 6 ] , &
iobj_1 [ 6 ] ) ; obj -> Bodies [ 7 ] = et05vlhztn ( & iobj_2 [ 7 ] , & iobj_0
[ 7 ] , & iobj_1 [ 7 ] ) ; obj -> Bodies [ 8 ] = oct3agbiik ( & iobj_2 [ 8 ]
, & iobj_0 [ 8 ] , & iobj_1 [ 8 ] ) ; iobj_2 = & obj -> _pobj2 [ 9 ] ; iobj_0
= & obj -> _pobj0 [ 9 ] ; b_kstr = iobj_2 -> NameInternal -> size [ 0 ] *
iobj_2 -> NameInternal -> size [ 1 ] ; iobj_2 -> NameInternal -> size [ 0 ] =
1 ; iobj_2 -> NameInternal -> size [ 1 ] = 5 ; e3n3mnwk4b ( iobj_2 ->
NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) {
iobj_2 -> NameInternal -> data [ b_kstr ] = tmp_p [ b_kstr ] ; } iobj_2 ->
ParentIndex = 0.0 ; iobj_2 -> MassInternal = 0.079119962 ; iobj_2 ->
CenterOfMassInternal [ 0 ] = 0.00030876154 ; iobj_2 -> CenterOfMassInternal [
1 ] = 0.0 ; iobj_2 -> CenterOfMassInternal [ 2 ] = - 0.00012176461 ; for (
b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { iobj_2 -> InertiaInternal [ b_kstr ]
= tmp_e [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) { iobj_2
-> SpatialInertia [ b_kstr ] = tmp_i [ b_kstr ] ; } obj -> _pobj1 [ 9 ] .
InTree = false ; for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj -> _pobj1
[ 9 ] . JointToParentTransform [ b_kstr ] = tmp_m [ b_kstr ] ; } for ( b_kstr
= 0 ; b_kstr < 16 ; b_kstr ++ ) { obj -> _pobj1 [ 9 ] . ChildToJointTransform
[ b_kstr ] = tmp_m [ b_kstr ] ; } b_kstr = obj -> _pobj1 [ 9 ] . NameInternal
-> size [ 0 ] * obj -> _pobj1 [ 9 ] . NameInternal -> size [ 1 ] ; obj ->
_pobj1 [ 9 ] . NameInternal -> size [ 0 ] = 1 ; obj -> _pobj1 [ 9 ] .
NameInternal -> size [ 1 ] = 11 ; e3n3mnwk4b ( obj -> _pobj1 [ 9 ] .
NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 11 ; b_kstr ++ ) { obj
-> _pobj1 [ 9 ] . NameInternal -> data [ b_kstr ] = tmp_g [ b_kstr ] ; }
b_kstr = obj -> _pobj1 [ 9 ] . Type -> size [ 0 ] * obj -> _pobj1 [ 9 ] .
Type -> size [ 1 ] ; obj -> _pobj1 [ 9 ] . Type -> size [ 0 ] = 1 ; obj ->
_pobj1 [ 9 ] . Type -> size [ 1 ] = 5 ; e3n3mnwk4b ( obj -> _pobj1 [ 9 ] .
Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { obj -> _pobj1
[ 9 ] . Type -> data [ b_kstr ] = tmp_j [ b_kstr ] ; } m2n1niwoq4 ( & jname ,
2 ) ; b_kstr = jname -> size [ 0 ] * jname -> size [ 1 ] ; jname -> size [ 0
] = 1 ; jname -> size [ 1 ] = obj -> _pobj1 [ 9 ] . Type -> size [ 1 ] ;
e3n3mnwk4b ( jname , b_kstr ) ; loop_ub = obj -> _pobj1 [ 9 ] . Type -> size
[ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) { jname -> data [
b_kstr ] = obj -> _pobj1 [ 9 ] . Type -> data [ b_kstr ] ; } for ( b_kstr = 0
; b_kstr < 8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_f [ b_kstr ] ; } b_bool =
false ; if ( jname -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( jname -> data [ b_kstr - 1 ] != b [ b_kstr -
1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool = true ; exitg1 =
1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0 ; } else { for (
b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_c [ b_kstr ] ; }
if ( jname -> size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if
( b_kstr - 1 < 9 ) { if ( jname -> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ]
) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ;
} } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1 ; } else { b_kstr =
- 1 ; } } switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [
2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for ( b_kstr = 0 ;
b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; }
poslim_data [ 0 ] = - 3.1415926535897931 ; poslim_data [ 1 ] =
3.1415926535897931 ; obj -> _pobj1 [ 9 ] . VelocityNumber = 1.0 ; obj ->
_pobj1 [ 9 ] . PositionNumber = 1.0 ; obj -> _pobj1 [ 9 ] . JointAxisInternal
[ 0 ] = 0.0 ; obj -> _pobj1 [ 9 ] . JointAxisInternal [ 1 ] = 0.0 ; obj ->
_pobj1 [ 9 ] . JointAxisInternal [ 2 ] = 1.0 ; break ; case 1 : tmp [ 0 ] = 0
; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] =
1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] =
tmp [ b_kstr ] ; } poslim_data [ 0 ] = - 0.5 ; poslim_data [ 1 ] = 0.5 ; obj
-> _pobj1 [ 9 ] . VelocityNumber = 1.0 ; obj -> _pobj1 [ 9 ] . PositionNumber
= 1.0 ; obj -> _pobj1 [ 9 ] . JointAxisInternal [ 0 ] = 0.0 ; obj -> _pobj1 [
9 ] . JointAxisInternal [ 1 ] = 0.0 ; obj -> _pobj1 [ 9 ] . JointAxisInternal
[ 2 ] = 1.0 ; break ; default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = 0 ; } poslim_data [ 0 ] = 0.0 ; poslim_data [ 1 ]
= 0.0 ; obj -> _pobj1 [ 9 ] . VelocityNumber = 0.0 ; obj -> _pobj1 [ 9 ] .
PositionNumber = 0.0 ; obj -> _pobj1 [ 9 ] . JointAxisInternal [ 0 ] = 0.0 ;
obj -> _pobj1 [ 9 ] . JointAxisInternal [ 1 ] = 0.0 ; obj -> _pobj1 [ 9 ] .
JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = obj -> _pobj1 [ 9 ] .
MotionSubspace -> size [ 0 ] * obj -> _pobj1 [ 9 ] . MotionSubspace -> size [
1 ] ; obj -> _pobj1 [ 9 ] . MotionSubspace -> size [ 0 ] = 6 ; obj -> _pobj1
[ 9 ] . MotionSubspace -> size [ 1 ] = 1 ; miyl1rq4uj ( obj -> _pobj1 [ 9 ] .
MotionSubspace , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { obj
-> _pobj1 [ 9 ] . MotionSubspace -> data [ b_kstr ] = msubspace_data [ b_kstr
] ; } b_kstr = obj -> _pobj1 [ 9 ] . PositionLimitsInternal -> size [ 0 ] *
obj -> _pobj1 [ 9 ] . PositionLimitsInternal -> size [ 1 ] ; obj -> _pobj1 [
9 ] . PositionLimitsInternal -> size [ 0 ] = 1 ; obj -> _pobj1 [ 9 ] .
PositionLimitsInternal -> size [ 1 ] = 2 ; miyl1rq4uj ( obj -> _pobj1 [ 9 ] .
PositionLimitsInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 2 ; b_kstr ++
) { obj -> _pobj1 [ 9 ] . PositionLimitsInternal -> data [ b_kstr ] =
poslim_data [ b_kstr ] ; } b_kstr = obj -> _pobj1 [ 9 ] .
HomePositionInternal -> size [ 0 ] ; obj -> _pobj1 [ 9 ] .
HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( obj -> _pobj1 [ 9 ] .
HomePositionInternal , b_kstr ) ; obj -> _pobj1 [ 9 ] . HomePositionInternal
-> data [ 0 ] = 0.0 ; iobj_2 -> JointInternal = & obj -> _pobj1 [ 9 ] ; for (
b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { iobj_2 -> JointInternal ->
JointToParentTransform [ b_kstr ] = tmp_k [ b_kstr ] ; } for ( b_kstr = 0 ;
b_kstr < 16 ; b_kstr ++ ) { iobj_2 -> JointInternal -> ChildToJointTransform
[ b_kstr ] = tmp_b [ b_kstr ] ; } b_kstr = iobj_2 -> JointInternal ->
MotionSubspace -> size [ 0 ] * iobj_2 -> JointInternal -> MotionSubspace ->
size [ 1 ] ; iobj_2 -> JointInternal -> MotionSubspace -> size [ 0 ] = 6 ;
iobj_2 -> JointInternal -> MotionSubspace -> size [ 1 ] = 1 ; miyl1rq4uj (
iobj_2 -> JointInternal -> MotionSubspace , b_kstr ) ; for ( b_kstr = 0 ;
b_kstr < 6 ; b_kstr ++ ) { iobj_2 -> JointInternal -> MotionSubspace -> data
[ b_kstr ] = 0.0 ; } iobj_2 -> JointInternal -> InTree = true ; b_kstr =
iobj_2 -> JointInternal -> PositionLimitsInternal -> size [ 0 ] * iobj_2 ->
JointInternal -> PositionLimitsInternal -> size [ 1 ] ; iobj_2 ->
JointInternal -> PositionLimitsInternal -> size [ 0 ] = 1 ; iobj_2 ->
JointInternal -> PositionLimitsInternal -> size [ 1 ] = 2 ; miyl1rq4uj (
iobj_2 -> JointInternal -> PositionLimitsInternal , b_kstr ) ; iobj_2 ->
JointInternal -> PositionLimitsInternal -> data [ 0 ] = 0.0 ; iobj_2 ->
JointInternal -> PositionLimitsInternal -> data [ iobj_2 -> JointInternal ->
PositionLimitsInternal -> size [ 0 ] ] = 0.0 ; iobj_2 -> JointInternal ->
JointAxisInternal [ 0 ] = 0.0 ; iobj_2 -> JointInternal -> JointAxisInternal
[ 1 ] = 0.0 ; iobj_2 -> JointInternal -> JointAxisInternal [ 2 ] = 0.0 ;
b_kstr = iobj_2 -> JointInternal -> HomePositionInternal -> size [ 0 ] ;
iobj_2 -> JointInternal -> HomePositionInternal -> size [ 0 ] = 1 ;
miyl1rq4uj ( iobj_2 -> JointInternal -> HomePositionInternal , b_kstr ) ;
iobj_2 -> JointInternal -> HomePositionInternal -> data [ 0 ] = 0.0 ; iobj_2
-> CollisionsInternal = p1kacjwirim ( iobj_0 , 0.0 ) ; iobj_2 ->
matlabCodegenIsDeleted = false ; obj -> Bodies [ 0 ] = iobj_2 ; obj -> Bodies
[ 0 ] -> Index = 1.0 ; iobj_2 = & obj -> _pobj2 [ 10 ] ; iobj_0 = & obj ->
_pobj0 [ 10 ] ; b_kstr = iobj_2 -> NameInternal -> size [ 0 ] * iobj_2 ->
NameInternal -> size [ 1 ] ; iobj_2 -> NameInternal -> size [ 0 ] = 1 ;
iobj_2 -> NameInternal -> size [ 1 ] = 5 ; e3n3mnwk4b ( iobj_2 ->
NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) {
iobj_2 -> NameInternal -> data [ b_kstr ] = tmp_n [ b_kstr ] ; } iobj_2 ->
ParentIndex = 1.0 ; iobj_2 -> MassInternal = 0.098406837 ; iobj_2 ->
CenterOfMassInternal [ 0 ] = - 0.0003018487 ; iobj_2 -> CenterOfMassInternal
[ 1 ] = 0.00054043684 ; iobj_2 -> CenterOfMassInternal [ 2 ] = 0.047433 ; for
( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { iobj_2 -> InertiaInternal [ b_kstr
] = tmp_l [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) {
iobj_2 -> SpatialInertia [ b_kstr ] = tmp_d [ b_kstr ] ; } obj -> _pobj1 [ 10
] . InTree = false ; for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj ->
_pobj1 [ 10 ] . JointToParentTransform [ b_kstr ] = tmp_m [ b_kstr ] ; } for
( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj -> _pobj1 [ 10 ] .
ChildToJointTransform [ b_kstr ] = tmp_m [ b_kstr ] ; } b_kstr = obj ->
_pobj1 [ 10 ] . NameInternal -> size [ 0 ] * obj -> _pobj1 [ 10 ] .
NameInternal -> size [ 1 ] ; obj -> _pobj1 [ 10 ] . NameInternal -> size [ 0
] = 1 ; obj -> _pobj1 [ 10 ] . NameInternal -> size [ 1 ] = 6 ; e3n3mnwk4b (
obj -> _pobj1 [ 10 ] . NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr <
6 ; b_kstr ++ ) { obj -> _pobj1 [ 10 ] . NameInternal -> data [ b_kstr ] =
tmp_o [ b_kstr ] ; } b_kstr = obj -> _pobj1 [ 10 ] . Type -> size [ 0 ] * obj
-> _pobj1 [ 10 ] . Type -> size [ 1 ] ; obj -> _pobj1 [ 10 ] . Type -> size [
0 ] = 1 ; obj -> _pobj1 [ 10 ] . Type -> size [ 1 ] = 8 ; e3n3mnwk4b ( obj ->
_pobj1 [ 10 ] . Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ )
{ obj -> _pobj1 [ 10 ] . Type -> data [ b_kstr ] = tmp_f [ b_kstr ] ; }
b_kstr = jname -> size [ 0 ] * jname -> size [ 1 ] ; jname -> size [ 0 ] = 1
; jname -> size [ 1 ] = obj -> _pobj1 [ 10 ] . Type -> size [ 1 ] ;
e3n3mnwk4b ( jname , b_kstr ) ; loop_ub = obj -> _pobj1 [ 10 ] . Type -> size
[ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) { jname -> data [
b_kstr ] = obj -> _pobj1 [ 10 ] . Type -> data [ b_kstr ] ; } b_bool = false
; if ( jname -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ;
if ( b_kstr - 1 < 8 ) { if ( jname -> data [ b_kstr - 1 ] != b [ b_kstr - 1 ]
) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ;
} } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0 ; } else { for (
b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_c [ b_kstr ] ; }
if ( jname -> size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if
( b_kstr - 1 < 9 ) { if ( jname -> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ]
) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ;
} } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1 ; } else { b_kstr =
- 1 ; } } switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [
2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for ( b_kstr = 0 ;
b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; }
poslim_data [ 0 ] = - 3.1415926535897931 ; poslim_data [ 1 ] =
3.1415926535897931 ; obj -> _pobj1 [ 10 ] . VelocityNumber = 1.0 ; obj ->
_pobj1 [ 10 ] . PositionNumber = 1.0 ; obj -> _pobj1 [ 10 ] .
JointAxisInternal [ 0 ] = 0.0 ; obj -> _pobj1 [ 10 ] . JointAxisInternal [ 1
] = 0.0 ; obj -> _pobj1 [ 10 ] . JointAxisInternal [ 2 ] = 1.0 ; break ; case
1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] = 0 ; tmp [ 4 ]
= 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } poslim_data [ 0 ] = - 0.5 ;
poslim_data [ 1 ] = 0.5 ; obj -> _pobj1 [ 10 ] . VelocityNumber = 1.0 ; obj
-> _pobj1 [ 10 ] . PositionNumber = 1.0 ; obj -> _pobj1 [ 10 ] .
JointAxisInternal [ 0 ] = 0.0 ; obj -> _pobj1 [ 10 ] . JointAxisInternal [ 1
] = 0.0 ; obj -> _pobj1 [ 10 ] . JointAxisInternal [ 2 ] = 1.0 ; break ;
default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [
b_kstr ] = 0 ; } poslim_data [ 0 ] = 0.0 ; poslim_data [ 1 ] = 0.0 ; obj ->
_pobj1 [ 10 ] . VelocityNumber = 0.0 ; obj -> _pobj1 [ 10 ] . PositionNumber
= 0.0 ; obj -> _pobj1 [ 10 ] . JointAxisInternal [ 0 ] = 0.0 ; obj -> _pobj1
[ 10 ] . JointAxisInternal [ 1 ] = 0.0 ; obj -> _pobj1 [ 10 ] .
JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = obj -> _pobj1 [ 10 ] .
MotionSubspace -> size [ 0 ] * obj -> _pobj1 [ 10 ] . MotionSubspace -> size
[ 1 ] ; obj -> _pobj1 [ 10 ] . MotionSubspace -> size [ 0 ] = 6 ; obj ->
_pobj1 [ 10 ] . MotionSubspace -> size [ 1 ] = 1 ; miyl1rq4uj ( obj -> _pobj1
[ 10 ] . MotionSubspace , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr
++ ) { obj -> _pobj1 [ 10 ] . MotionSubspace -> data [ b_kstr ] =
msubspace_data [ b_kstr ] ; } b_kstr = obj -> _pobj1 [ 10 ] .
PositionLimitsInternal -> size [ 0 ] * obj -> _pobj1 [ 10 ] .
PositionLimitsInternal -> size [ 1 ] ; obj -> _pobj1 [ 10 ] .
PositionLimitsInternal -> size [ 0 ] = 1 ; obj -> _pobj1 [ 10 ] .
PositionLimitsInternal -> size [ 1 ] = 2 ; miyl1rq4uj ( obj -> _pobj1 [ 10 ]
. PositionLimitsInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 2 ; b_kstr
++ ) { obj -> _pobj1 [ 10 ] . PositionLimitsInternal -> data [ b_kstr ] =
poslim_data [ b_kstr ] ; } b_kstr = obj -> _pobj1 [ 10 ] .
HomePositionInternal -> size [ 0 ] ; obj -> _pobj1 [ 10 ] .
HomePositionInternal -> size [ 0 ] = 1 ; miyl1rq4uj ( obj -> _pobj1 [ 10 ] .
HomePositionInternal , b_kstr ) ; obj -> _pobj1 [ 10 ] . HomePositionInternal
-> data [ 0 ] = 0.0 ; iobj_2 -> JointInternal = & obj -> _pobj1 [ 10 ] ; for
( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { iobj_2 -> JointInternal ->
JointToParentTransform [ b_kstr ] = tmp_dz [ b_kstr ] ; } for ( b_kstr = 0 ;
b_kstr < 16 ; b_kstr ++ ) { iobj_2 -> JointInternal -> ChildToJointTransform
[ b_kstr ] = tmp_b [ b_kstr ] ; } b_kstr = iobj_2 -> JointInternal ->
MotionSubspace -> size [ 0 ] * iobj_2 -> JointInternal -> MotionSubspace ->
size [ 1 ] ; iobj_2 -> JointInternal -> MotionSubspace -> size [ 0 ] = 6 ;
iobj_2 -> JointInternal -> MotionSubspace -> size [ 1 ] = 1 ; miyl1rq4uj (
iobj_2 -> JointInternal -> MotionSubspace , b_kstr ) ; for ( b_kstr = 0 ;
b_kstr < 6 ; b_kstr ++ ) { iobj_2 -> JointInternal -> MotionSubspace -> data
[ b_kstr ] = tmp_fs [ b_kstr ] ; } iobj_2 -> JointInternal -> InTree = true ;
b_kstr = iobj_2 -> JointInternal -> PositionLimitsInternal -> size [ 0 ] *
iobj_2 -> JointInternal -> PositionLimitsInternal -> size [ 1 ] ; iobj_2 ->
JointInternal -> PositionLimitsInternal -> size [ 0 ] = 1 ; iobj_2 ->
JointInternal -> PositionLimitsInternal -> size [ 1 ] = 2 ; miyl1rq4uj (
iobj_2 -> JointInternal -> PositionLimitsInternal , b_kstr ) ; iobj_2 ->
JointInternal -> PositionLimitsInternal -> data [ 0 ] = - 2.8274 ; iobj_2 ->
JointInternal -> PositionLimitsInternal -> data [ iobj_2 -> JointInternal ->
PositionLimitsInternal -> size [ 0 ] ] = 2.8274 ; iobj_2 -> JointInternal ->
JointAxisInternal [ 0 ] = 0.0 ; iobj_2 -> JointInternal -> JointAxisInternal
[ 1 ] = 0.0 ; iobj_2 -> JointInternal -> JointAxisInternal [ 2 ] = 1.0 ;
b_kstr = iobj_2 -> JointInternal -> HomePositionInternal -> size [ 0 ] ;
iobj_2 -> JointInternal -> HomePositionInternal -> size [ 0 ] = 1 ;
miyl1rq4uj ( iobj_2 -> JointInternal -> HomePositionInternal , b_kstr ) ;
iobj_2 -> JointInternal -> HomePositionInternal -> data [ 0 ] = 0.0 ; iobj_2
-> CollisionsInternal = p1kacjwirim ( iobj_0 , 0.0 ) ; iobj_2 ->
matlabCodegenIsDeleted = false ; obj -> Bodies [ 1 ] = iobj_2 ; obj -> Bodies
[ 1 ] -> Index = 2.0 ; obj -> Bodies [ 2 ] = jphupobpz2 ( & obj -> _pobj2 [
11 ] , & obj -> _pobj0 [ 11 ] , & obj -> _pobj1 [ 11 ] ) ; obj -> Bodies [ 2
] -> Index = 3.0 ; obj -> Bodies [ 3 ] = bl5mnhfgqx ( & obj -> _pobj2 [ 12 ]
, & obj -> _pobj0 [ 12 ] , & obj -> _pobj1 [ 12 ] ) ; obj -> Bodies [ 3 ] ->
Index = 4.0 ; obj -> Bodies [ 4 ] = gwskexfjch ( & obj -> _pobj2 [ 13 ] , &
obj -> _pobj0 [ 13 ] , & obj -> _pobj1 [ 13 ] ) ; obj -> Bodies [ 4 ] ->
Index = 5.0 ; obj -> Bodies [ 5 ] = nuiglfj4n2 ( & obj -> _pobj2 [ 14 ] , &
obj -> _pobj0 [ 14 ] , & obj -> _pobj1 [ 14 ] ) ; obj -> Bodies [ 5 ] ->
Index = 6.0 ; obj -> Bodies [ 6 ] = nxwlc1tqzg ( & obj -> _pobj2 [ 15 ] , &
obj -> _pobj0 [ 15 ] , & obj -> _pobj1 [ 15 ] ) ; obj -> Bodies [ 6 ] ->
Index = 7.0 ; obj -> Bodies [ 7 ] = gtyzt122qi ( & obj -> _pobj2 [ 16 ] , &
obj -> _pobj0 [ 16 ] , & obj -> _pobj1 [ 16 ] ) ; obj -> Bodies [ 7 ] ->
Index = 8.0 ; obj -> Bodies [ 8 ] = dyhbwd1qgw ( & obj -> _pobj2 [ 17 ] , &
obj -> _pobj0 [ 17 ] , & obj -> _pobj1 [ 17 ] ) ; obj -> Bodies [ 8 ] ->
Index = 9.0 ; obj -> Gravity [ 0 ] = 0.0 ; obj -> Gravity [ 1 ] = 0.0 ; obj
-> Gravity [ 2 ] = - 9.80665 ; iobj_2 = & obj -> Base ; iobj_0 = & obj ->
_pobj0 [ 18 ] ; b_kstr = iobj_2 -> NameInternal -> size [ 0 ] * iobj_2 ->
NameInternal -> size [ 1 ] ; iobj_2 -> NameInternal -> size [ 0 ] = 1 ;
iobj_2 -> NameInternal -> size [ 1 ] = 5 ; e3n3mnwk4b ( iobj_2 ->
NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) {
iobj_2 -> NameInternal -> data [ b_kstr ] = tmp_ck [ b_kstr ] ; } iobj_2 ->
ParentIndex = - 1.0 ; iobj_2 -> MassInternal = 0.0 ; iobj_2 ->
CenterOfMassInternal [ 0 ] = 0.0 ; iobj_2 -> CenterOfMassInternal [ 1 ] = 0.0
; iobj_2 -> CenterOfMassInternal [ 2 ] = 0.0 ; for ( b_kstr = 0 ; b_kstr < 9
; b_kstr ++ ) { iobj_2 -> InertiaInternal [ b_kstr ] = 0.0 ; } for ( b_kstr =
0 ; b_kstr < 36 ; b_kstr ++ ) { iobj_2 -> SpatialInertia [ b_kstr ] = 0.0 ; }
b_kstr = jname -> size [ 0 ] * jname -> size [ 1 ] ; jname -> size [ 0 ] = 1
; jname -> size [ 1 ] = iobj_2 -> NameInternal -> size [ 1 ] + 4 ; e3n3mnwk4b
( jname , b_kstr ) ; loop_ub = iobj_2 -> NameInternal -> size [ 1 ] ; for (
b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) { jname -> data [ b_kstr ] =
iobj_2 -> NameInternal -> data [ b_kstr ] ; } jname -> data [ iobj_2 ->
NameInternal -> size [ 1 ] ] = '_' ; jname -> data [ iobj_2 -> NameInternal
-> size [ 1 ] + 1 ] = 'j' ; jname -> data [ iobj_2 -> NameInternal -> size [
1 ] + 2 ] = 'n' ; jname -> data [ iobj_2 -> NameInternal -> size [ 1 ] + 3 ]
= 't' ; iobj_2 -> JointInternal = g1japxta3w ( & obj -> _pobj1 [ 18 ] , jname
) ; fagcrr3avn ( & jname ) ; iobj_2 -> CollisionsInternal = p1kacjwirim (
iobj_0 , 0.0 ) ; iobj_2 -> matlabCodegenIsDeleted = false ; obj -> Base .
Index = 0.0 ; obj -> matlabCodegenIsDeleted = false ; return b_obj ; } static
void csqey0xxgza ( uint32_T mt [ 625 ] , uint32_T u [ 2 ] ) { int32_T b_j ;
int32_T b_kk ; uint32_T mti ; uint32_T y ; for ( b_j = 0 ; b_j < 2 ; b_j ++ )
{ mti = mt [ 624 ] + 1U ; if ( mt [ 624 ] + 1U >= 625U ) { for ( b_kk = 0 ;
b_kk < 227 ; b_kk ++ ) { y = ( mt [ b_kk + 1 ] & 2147483647U ) | ( mt [ b_kk
] & 2147483648U ) ; if ( ( y & 1U ) == 0U ) { mti = y >> 1U ; } else { mti =
y >> 1U ^ 2567483615U ; } mt [ b_kk ] = mt [ b_kk + 397 ] ^ mti ; } for (
b_kk = 0 ; b_kk < 396 ; b_kk ++ ) { y = ( mt [ b_kk + 227 ] & 2147483648U ) |
( mt [ b_kk + 228 ] & 2147483647U ) ; if ( ( y & 1U ) == 0U ) { mti = y >> 1U
; } else { mti = y >> 1U ^ 2567483615U ; } mt [ b_kk + 227 ] = mt [ b_kk ] ^
mti ; } y = ( mt [ 623 ] & 2147483648U ) | ( mt [ 0 ] & 2147483647U ) ; if (
( y & 1U ) == 0U ) { mti = y >> 1U ; } else { mti = y >> 1U ^ 2567483615U ; }
mt [ 623 ] = mt [ 396 ] ^ mti ; mti = 1U ; } y = mt [ ( int32_T ) mti - 1 ] ;
mt [ 624 ] = mti ; y ^= y >> 11U ; y ^= y << 7U & 2636928640U ; y ^= y << 15U
& 4022730752U ; u [ b_j ] = y >> 18U ^ y ; } } static boolean_T enplvw3xkz (
const uint32_T mt [ 625 ] ) { int32_T k ; boolean_T exitg1 ; boolean_T
isvalid ; if ( ( mt [ 624 ] >= 1U ) && ( mt [ 624 ] < 625U ) ) { isvalid =
false ; k = 0 ; exitg1 = false ; while ( ( ! exitg1 ) && ( k + 1 < 625 ) ) {
if ( mt [ k ] == 0U ) { k ++ ; } else { isvalid = true ; exitg1 = true ; } }
} else { isvalid = false ; } return isvalid ; } static void jlodq4k5z5 (
real_T r [ 5 ] ) { real_T b_r ; int32_T b_k ; int32_T exitg1 ; uint32_T b_u [
2 ] ; for ( b_k = 0 ; b_k < 5 ; b_k ++ ) { do { exitg1 = 0 ; csqey0xxgza (
rtDW . ehmyrdjr0v , b_u ) ; b_r = ( ( real_T ) ( b_u [ 0 ] >> 5U ) *
6.7108864E+7 + ( real_T ) ( b_u [ 1 ] >> 6U ) ) * 1.1102230246251565E-16 ; if
( b_r == 0.0 ) { if ( ! enplvw3xkz ( rtDW . ehmyrdjr0v ) ) { rtDW .
ehmyrdjr0v [ 0 ] = 5489U ; rtDW . ehmyrdjr0v [ 624 ] = 624U ; } } else {
exitg1 = 1 ; } } while ( exitg1 == 0 ) ; r [ b_k ] = b_r ; } } static
boolean_T e3fq41hhh0 ( const e1n154mvbh * a , const e1n154mvbh * b ) {
int32_T b_kstr ; int32_T exitg1 ; boolean_T b_bool ; boolean_T d ; b_bool =
false ; d = ( a -> size [ 1 ] == 0 ) ; if ( d && ( b -> size [ 1 ] == 0 ) ) {
b_bool = true ; } else if ( a -> size [ 1 ] != b -> size [ 1 ] ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 <= b -> size [ 1 ] - 1 ) { if
( a -> data [ b_kstr - 1 ] != b -> data [ b_kstr - 1 ] ) { exitg1 = 1 ; }
else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1
== 0 ) ; } return b_bool ; } static real_T jjyjgxfdua ( m0caqfktx0 * obj ,
const e1n154mvbh * bodyname ) { alyuhn31fbfz * obj_p ; e1n154mvbh * bname ;
real_T b ; real_T bid ; int32_T b_i ; int32_T i ; int32_T loop_ub ; boolean_T
exitg1 ; bid = - 1.0 ; m2n1niwoq4 ( & bname , 2 ) ; i = bname -> size [ 0 ] *
bname -> size [ 1 ] ; bname -> size [ 0 ] = 1 ; bname -> size [ 1 ] = obj ->
Base . NameInternal -> size [ 1 ] ; e3n3mnwk4b ( bname , i ) ; loop_ub = obj
-> Base . NameInternal -> size [ 1 ] ; for ( i = 0 ; i < loop_ub ; i ++ ) {
bname -> data [ i ] = obj -> Base . NameInternal -> data [ i ] ; } if (
e3fq41hhh0 ( bname , bodyname ) ) { bid = 0.0 ; } else { b = obj -> NumBodies
; b_i = 0 ; exitg1 = false ; while ( ( ! exitg1 ) && ( b_i <= ( int32_T ) b -
1 ) ) { obj_p = obj -> Bodies [ b_i ] ; i = bname -> size [ 0 ] * bname ->
size [ 1 ] ; bname -> size [ 0 ] = 1 ; bname -> size [ 1 ] = obj_p ->
NameInternal -> size [ 1 ] ; e3n3mnwk4b ( bname , i ) ; loop_ub = obj_p ->
NameInternal -> size [ 1 ] ; for ( i = 0 ; i < loop_ub ; i ++ ) { bname ->
data [ i ] = obj_p -> NameInternal -> data [ i ] ; } if ( e3fq41hhh0 ( bname
, bodyname ) ) { bid = ( real_T ) b_i + 1.0 ; exitg1 = true ; } else { b_i ++
; } } } fagcrr3avn ( & bname ) ; return bid ; } static void bdhoypqe5x (
klgljlxbmf * * pEmxArray ) { if ( * pEmxArray != ( klgljlxbmf * ) NULL ) { if
( ( ( * pEmxArray ) -> data != ( real_T * ) NULL ) && ( * pEmxArray ) ->
canFreeData ) { free ( ( * pEmxArray ) -> data ) ; } free ( ( * pEmxArray )
-> size ) ; free ( * pEmxArray ) ; * pEmxArray = ( klgljlxbmf * ) NULL ; } }
static alyuhn31fbfz * pfo1apkwc0 ( alyuhn31fbfz * obj , ogx4llc4arl * iobj_0
, ot4uovzkt0w * iobj_1 , alyuhn31fbfz * iobj_2 ) { void *
obj_CollisionPrimitive ; alyuhn31fbfz * newbody ; e1n154mvbh * jname ;
e1n154mvbh * jtype ; klgljlxbmf * obj_i ; ogx4llc4arl * newObj ; ogx4llc4arl
* obj_e ; ot4uovzkt0w * obj_p ; real_T obj_j [ 36 ] ; real_T obj_WorldPose [
16 ] ; real_T obj_m [ 16 ] ; real_T poslim_data [ 12 ] ; real_T obj_g [ 9 ] ;
real_T b_x ; real_T obj_idx_1 ; real_T obj_idx_2 ; int32_T b_k ; int32_T
b_kstr ; int32_T minnanb ; char_T b_b_p [ 9 ] ; char_T b_vstr [ 9 ] ; char_T
partial_match_data [ 9 ] ; char_T b_b [ 8 ] ; char_T vstr [ 8 ] ; char_T
b_b_e [ 5 ] ; char_T c_vstr [ 5 ] ; int8_T msubspace_data [ 36 ] ; int8_T b_I
[ 9 ] ; int8_T tmp [ 6 ] ; boolean_T b_bool ; boolean_T matched ; static
const int8_T tmp_p [ 16 ] = { 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 , 0 ,
0 , 0 , 0 , 1 } ; static const char_T tmp_e [ 8 ] = { 'r' , 'e' , 'v' , 'o' ,
'l' , 'u' , 't' , 'e' } ; static const char_T tmp_i [ 9 ] = { 'p' , 'r' , 'i'
, 's' , 'm' , 'a' , 't' , 'i' , 'c' } ; static const char_T tmp_m [ 5 ] = {
'f' , 'i' , 'x' , 'e' , 'd' } ; static const char_T tmp_g [ 128 ] = { '\x00'
, '\x01' , '\x02' , '\x03' , '\x04' , '\x05' , '\x06' , '\x07' , '\x08' ,
'\x09' , '\x0a' , '\x0b' , '\x0c' , '\x0d' , '\x0e' , '\x0f' , '\x10' ,
'\x11' , '\x12' , '\x13' , '\x14' , '\x15' , '\x16' , '\x17' , '\x18' ,
'\x19' , '\x1a' , '\x1b' , '\x1c' , '\x1d' , '\x1e' , '\x1f' , ' ' , '!' ,
'\"' , '#' , '$' , '%' , '&' , '\'' , '(' , ')' , '*' , '+' , ',' , '-' , '.'
, '/' , '0' , '1' , '2' , '3' , '4' , '5' , '6' , '7' , '8' , '9' , ':' , ';'
, '<' , '=' , '>' , '?' , '@' , 'a' , 'b' , 'c' , 'd' , 'e' , 'f' , 'g' , 'h'
, 'i' , 'j' , 'k' , 'l' , 'm' , 'n' , 'o' , 'p' , 'q' , 'r' , 's' , 't' , 'u'
, 'v' , 'w' , 'x' , 'y' , 'z' , '[' , '\\' , ']' , '^' , '_' , '`' , 'a' ,
'b' , 'c' , 'd' , 'e' , 'f' , 'g' , 'h' , 'i' , 'j' , 'k' , 'l' , 'm' , 'n' ,
'o' , 'p' , 'q' , 'r' , 's' , 't' , 'u' , 'v' , 'w' , 'x' , 'y' , 'z' , '{' ,
'|' , '}' , '~' , '\x7f' } ; int32_T exitg1 ; int32_T
partial_match_size_idx_1 ; boolean_T guard1 = false ; boolean_T guard11 =
false ; boolean_T guard2 = false ; boolean_T guard3 = false ; m2n1niwoq4 ( &
jtype , 2 ) ; b_kstr = jtype -> size [ 0 ] * jtype -> size [ 1 ] ; jtype ->
size [ 0 ] = 1 ; jtype -> size [ 1 ] = obj -> NameInternal -> size [ 1 ] ;
e3n3mnwk4b ( jtype , b_kstr ) ; minnanb = obj -> NameInternal -> size [ 1 ] ;
for ( b_kstr = 0 ; b_kstr < minnanb ; b_kstr ++ ) { jtype -> data [ b_kstr ]
= obj -> NameInternal -> data [ b_kstr ] ; } newbody = iobj_2 ; b_kstr =
iobj_2 -> NameInternal -> size [ 0 ] * iobj_2 -> NameInternal -> size [ 1 ] ;
iobj_2 -> NameInternal -> size [ 0 ] = 1 ; iobj_2 -> NameInternal -> size [ 1
] = jtype -> size [ 1 ] ; e3n3mnwk4b ( iobj_2 -> NameInternal , b_kstr ) ;
minnanb = jtype -> size [ 1 ] - 1 ; for ( b_kstr = 0 ; b_kstr <= minnanb ;
b_kstr ++ ) { iobj_2 -> NameInternal -> data [ b_kstr ] = jtype -> data [
b_kstr ] ; } m2n1niwoq4 ( & jname , 2 ) ; b_kstr = jname -> size [ 0 ] *
jname -> size [ 1 ] ; jname -> size [ 0 ] = 1 ; jname -> size [ 1 ] = jtype
-> size [ 1 ] + 4 ; e3n3mnwk4b ( jname , b_kstr ) ; minnanb = jtype -> size [
1 ] ; if ( minnanb - 1 >= 0 ) { memcpy ( & jname -> data [ 0 ] , & jtype ->
data [ 0 ] , ( uint32_T ) minnanb * sizeof ( char_T ) ) ; } jname -> data [
jtype -> size [ 1 ] ] = '_' ; jname -> data [ jtype -> size [ 1 ] + 1 ] = 'j'
; jname -> data [ jtype -> size [ 1 ] + 2 ] = 'n' ; jname -> data [ jtype ->
size [ 1 ] + 3 ] = 't' ; iobj_2 -> JointInternal = g1japxta3w ( & iobj_1 [ 0
] , jname ) ; iobj_2 -> Index = - 1.0 ; iobj_2 -> ParentIndex = - 1.0 ;
iobj_2 -> MassInternal = 1.0 ; iobj_2 -> CenterOfMassInternal [ 0 ] = 0.0 ;
iobj_2 -> CenterOfMassInternal [ 1 ] = 0.0 ; iobj_2 -> CenterOfMassInternal [
2 ] = 0.0 ; for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_I [ b_kstr ] = 0
; } b_I [ 0 ] = 1 ; b_I [ 4 ] = 1 ; b_I [ 8 ] = 1 ; for ( b_kstr = 0 ; b_kstr
< 9 ; b_kstr ++ ) { iobj_2 -> InertiaInternal [ b_kstr ] = b_I [ b_kstr ] ; }
for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = 0
; } for ( b_k = 0 ; b_k < 6 ; b_k ++ ) { msubspace_data [ b_k + 6 * b_k ] = 1
; } for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) { iobj_2 -> SpatialInertia [
b_kstr ] = msubspace_data [ b_kstr ] ; } iobj_2 -> CollisionsInternal =
p1kacjwirim ( & iobj_0 [ 0 ] , 0.0 ) ; iobj_2 -> matlabCodegenIsDeleted =
false ; obj_p = obj -> JointInternal ; b_kstr = jtype -> size [ 0 ] * jtype
-> size [ 1 ] ; jtype -> size [ 0 ] = 1 ; jtype -> size [ 1 ] = obj_p -> Type
-> size [ 1 ] ; e3n3mnwk4b ( jtype , b_kstr ) ; minnanb = obj_p -> Type ->
size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < minnanb ; b_kstr ++ ) { jtype ->
data [ b_kstr ] = obj_p -> Type -> data [ b_kstr ] ; } b_kstr = jname -> size
[ 0 ] * jname -> size [ 1 ] ; jname -> size [ 0 ] = 1 ; jname -> size [ 1 ] =
obj_p -> NameInternal -> size [ 1 ] ; e3n3mnwk4b ( jname , b_kstr ) ; minnanb
= obj_p -> NameInternal -> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < minnanb ;
b_kstr ++ ) { jname -> data [ b_kstr ] = obj_p -> NameInternal -> data [
b_kstr ] ; } iobj_1 [ 1 ] . InTree = false ; for ( b_kstr = 0 ; b_kstr < 16 ;
b_kstr ++ ) { iobj_1 [ 1 ] . JointToParentTransform [ b_kstr ] = tmp_p [
b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { iobj_1 [ 1 ] .
ChildToJointTransform [ b_kstr ] = tmp_p [ b_kstr ] ; } b_kstr = iobj_1 [ 1 ]
. NameInternal -> size [ 0 ] * iobj_1 [ 1 ] . NameInternal -> size [ 1 ] ;
iobj_1 [ 1 ] . NameInternal -> size [ 0 ] = 1 ; iobj_1 [ 1 ] . NameInternal
-> size [ 1 ] = jname -> size [ 1 ] ; e3n3mnwk4b ( iobj_1 [ 1 ] .
NameInternal , b_kstr ) ; minnanb = jname -> size [ 1 ] - 1 ; for ( b_kstr =
0 ; b_kstr <= minnanb ; b_kstr ++ ) { iobj_1 [ 1 ] . NameInternal -> data [
b_kstr ] = jname -> data [ b_kstr ] ; } fagcrr3avn ( & jname ) ;
partial_match_size_idx_1 = 8 ; b_k = 0 ; matched = false ; for ( b_kstr = 0 ;
b_kstr < 8 ; b_kstr ++ ) { partial_match_data [ b_kstr ] = ' ' ; vstr [
b_kstr ] = tmp_e [ b_kstr ] ; } guard1 = false ; guard2 = false ; guard3 =
false ; if ( jtype -> size [ 1 ] <= 8 ) { b_x = jtype -> size [ 1 ] ; for (
b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { b_b [ b_kstr ] = tmp_e [ b_kstr ] ; }
b_bool = false ; minnanb = jtype -> size [ 1 ] ; guard11 = false ; if ( (
int32_T ) b_x <= minnanb ) { b_kstr = ( int32_T ) b_x ; minnanb =
muIntScalarMin_sint32 ( minnanb , b_kstr ) ; guard11 = true ; } else if (
jtype -> size [ 1 ] == 8 ) { minnanb = 8 ; guard11 = true ; } if ( guard11 )
{ b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 <= minnanb - 1 ) { if (
tmp_g [ ( uint8_T ) jtype -> data [ b_kstr - 1 ] & 127 ] != tmp_g [ ( int32_T
) b_b [ b_kstr - 1 ] ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else {
b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { if
( jtype -> size [ 1 ] == 8 ) { b_k = 1 ; partial_match_size_idx_1 = 8 ; for (
b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { b_b_p [ b_kstr ] = vstr [ b_kstr ] ;
} } else { partial_match_size_idx_1 = 8 ; for ( b_kstr = 0 ; b_kstr < 8 ;
b_kstr ++ ) { partial_match_data [ b_kstr ] = vstr [ b_kstr ] ; } matched =
true ; b_k = 1 ; guard3 = true ; } } else { guard3 = true ; } } else { guard3
= true ; } if ( guard3 ) { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) {
b_vstr [ b_kstr ] = tmp_i [ b_kstr ] ; } if ( jtype -> size [ 1 ] <= 9 ) {
b_x = jtype -> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) {
b_b_p [ b_kstr ] = tmp_i [ b_kstr ] ; } b_bool = false ; minnanb = jtype ->
size [ 1 ] ; guard11 = false ; if ( ( int32_T ) b_x <= minnanb ) { b_kstr = (
int32_T ) b_x ; minnanb = muIntScalarMin_sint32 ( minnanb , b_kstr ) ;
guard11 = true ; } else if ( jtype -> size [ 1 ] == 9 ) { minnanb = 9 ;
guard11 = true ; } if ( guard11 ) { b_kstr = 1 ; do { exitg1 = 0 ; if (
b_kstr - 1 <= minnanb - 1 ) { if ( tmp_g [ ( uint8_T ) jtype -> data [ b_kstr
- 1 ] & 127 ] != tmp_g [ ( int32_T ) b_b_p [ b_kstr - 1 ] ] ) { exitg1 = 1 ;
} else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { if ( jtype -> size [ 1 ] == 9 ) { b_k = 1 ;
partial_match_size_idx_1 = 9 ; for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) {
b_b_p [ b_kstr ] = b_vstr [ b_kstr ] ; } } else { if ( ! matched ) {
partial_match_size_idx_1 = 9 ; for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) {
partial_match_data [ b_kstr ] = b_vstr [ b_kstr ] ; } } matched = true ; b_k
++ ; guard2 = true ; } } else { guard2 = true ; } } else { guard2 = true ; }
} if ( guard2 ) { for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { c_vstr [
b_kstr ] = tmp_m [ b_kstr ] ; } if ( jtype -> size [ 1 ] <= 5 ) { b_x = jtype
-> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { b_b_e [ b_kstr
] = tmp_m [ b_kstr ] ; } b_bool = false ; minnanb = jtype -> size [ 1 ] ;
guard11 = false ; if ( ( int32_T ) b_x <= minnanb ) { b_kstr = ( int32_T )
b_x ; minnanb = muIntScalarMin_sint32 ( minnanb , b_kstr ) ; guard11 = true ;
} else if ( jtype -> size [ 1 ] == 5 ) { minnanb = 5 ; guard11 = true ; } if
( guard11 ) { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 <= minnanb - 1 )
{ if ( tmp_g [ ( uint8_T ) jtype -> data [ b_kstr - 1 ] & 127 ] != tmp_g [ (
int32_T ) b_b_e [ b_kstr - 1 ] ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } }
else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool
) { if ( jtype -> size [ 1 ] == 5 ) { b_k = 1 ; partial_match_size_idx_1 = 5
; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { b_b_p [ b_kstr ] = c_vstr [
b_kstr ] ; } } else { if ( ! matched ) { partial_match_size_idx_1 = 5 ; for (
b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { partial_match_data [ b_kstr ] =
c_vstr [ b_kstr ] ; } } b_k ++ ; guard1 = true ; } } else { guard1 = true ; }
} else { guard1 = true ; } } if ( guard1 ) { if ( b_k == 0 ) {
partial_match_size_idx_1 = 8 ; for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) {
b_b_p [ b_kstr ] = ' ' ; } } else { memcpy ( & b_b_p [ 0 ] , &
partial_match_data [ 0 ] , ( uint32_T ) partial_match_size_idx_1 * sizeof (
char_T ) ) ; } } if ( ( b_k == 0 ) || ( jtype -> size [ 1 ] == 0 ) ) {
partial_match_size_idx_1 = 8 ; for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) {
partial_match_data [ b_kstr ] = ' ' ; } } else { memcpy ( &
partial_match_data [ 0 ] , & b_b_p [ 0 ] , ( uint32_T )
partial_match_size_idx_1 * sizeof ( char_T ) ) ; } b_kstr = iobj_1 [ 1 ] .
Type -> size [ 0 ] * iobj_1 [ 1 ] . Type -> size [ 1 ] ; iobj_1 [ 1 ] . Type
-> size [ 0 ] = 1 ; iobj_1 [ 1 ] . Type -> size [ 1 ] =
partial_match_size_idx_1 ; e3n3mnwk4b ( iobj_1 [ 1 ] . Type , b_kstr ) ;
minnanb = partial_match_size_idx_1 - 1 ; for ( b_kstr = 0 ; b_kstr <= minnanb
; b_kstr ++ ) { iobj_1 [ 1 ] . Type -> data [ b_kstr ] = partial_match_data [
b_kstr ] ; } b_kstr = jtype -> size [ 0 ] * jtype -> size [ 1 ] ; jtype ->
size [ 0 ] = 1 ; jtype -> size [ 1 ] = iobj_1 [ 1 ] . Type -> size [ 1 ] ;
e3n3mnwk4b ( jtype , b_kstr ) ; minnanb = iobj_1 [ 1 ] . Type -> size [ 1 ] ;
for ( b_kstr = 0 ; b_kstr < minnanb ; b_kstr ++ ) { jtype -> data [ b_kstr ]
= iobj_1 [ 1 ] . Type -> data [ b_kstr ] ; } b_bool = false ; if ( jtype ->
size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 <
8 ) { if ( jtype -> data [ b_kstr - 1 ] != vstr [ b_kstr - 1 ] ) { exitg1 = 1
; } else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { b_x = 0.0 ; } else { for ( b_kstr = 0 ;
b_kstr < 9 ; b_kstr ++ ) { b_vstr [ b_kstr ] = tmp_i [ b_kstr ] ; } if (
jtype -> size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if (
b_kstr - 1 < 9 ) { if ( jtype -> data [ b_kstr - 1 ] != b_vstr [ b_kstr - 1 ]
) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ;
} } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_x = 1.0 ; } else { b_x = -
1.0 ; } } switch ( ( int32_T ) b_x ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0
; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } poslim_data [ 0 ] = - 3.1415926535897931 ; poslim_data [ 1 ] =
3.1415926535897931 ; iobj_1 [ 1 ] . VelocityNumber = 1.0 ; iobj_1 [ 1 ] .
PositionNumber = 1.0 ; iobj_1 [ 1 ] . JointAxisInternal [ 0 ] = 0.0 ; iobj_1
[ 1 ] . JointAxisInternal [ 1 ] = 0.0 ; iobj_1 [ 1 ] . JointAxisInternal [ 2
] = 1.0 ; break ; case 1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ;
tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6
; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } poslim_data [
0 ] = - 0.5 ; poslim_data [ 1 ] = 0.5 ; iobj_1 [ 1 ] . VelocityNumber = 1.0 ;
iobj_1 [ 1 ] . PositionNumber = 1.0 ; iobj_1 [ 1 ] . JointAxisInternal [ 0 ]
= 0.0 ; iobj_1 [ 1 ] . JointAxisInternal [ 1 ] = 0.0 ; iobj_1 [ 1 ] .
JointAxisInternal [ 2 ] = 1.0 ; break ; default : for ( b_kstr = 0 ; b_kstr <
6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = 0 ; } poslim_data [ 0 ] = 0.0 ;
poslim_data [ 1 ] = 0.0 ; iobj_1 [ 1 ] . VelocityNumber = 0.0 ; iobj_1 [ 1 ]
. PositionNumber = 0.0 ; iobj_1 [ 1 ] . JointAxisInternal [ 0 ] = 0.0 ;
iobj_1 [ 1 ] . JointAxisInternal [ 1 ] = 0.0 ; iobj_1 [ 1 ] .
JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = iobj_1 [ 1 ] .
MotionSubspace -> size [ 0 ] * iobj_1 [ 1 ] . MotionSubspace -> size [ 1 ] ;
iobj_1 [ 1 ] . MotionSubspace -> size [ 0 ] = 6 ; iobj_1 [ 1 ] .
MotionSubspace -> size [ 1 ] = 1 ; miyl1rq4uj ( iobj_1 [ 1 ] . MotionSubspace
, b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { iobj_1 [ 1 ] .
MotionSubspace -> data [ b_kstr ] = msubspace_data [ b_kstr ] ; } b_kstr =
iobj_1 [ 1 ] . PositionLimitsInternal -> size [ 0 ] * iobj_1 [ 1 ] .
PositionLimitsInternal -> size [ 1 ] ; iobj_1 [ 1 ] . PositionLimitsInternal
-> size [ 0 ] = 1 ; iobj_1 [ 1 ] . PositionLimitsInternal -> size [ 1 ] = 2 ;
miyl1rq4uj ( iobj_1 [ 1 ] . PositionLimitsInternal , b_kstr ) ; for ( b_kstr
= 0 ; b_kstr < 2 ; b_kstr ++ ) { iobj_1 [ 1 ] . PositionLimitsInternal ->
data [ b_kstr ] = poslim_data [ b_kstr ] ; } b_kstr = iobj_1 [ 1 ] .
HomePositionInternal -> size [ 0 ] ; iobj_1 [ 1 ] . HomePositionInternal ->
size [ 0 ] = 1 ; miyl1rq4uj ( iobj_1 [ 1 ] . HomePositionInternal , b_kstr )
; iobj_1 [ 1 ] . HomePositionInternal -> data [ 0 ] = 0.0 ; b_kstr = jtype ->
size [ 0 ] * jtype -> size [ 1 ] ; jtype -> size [ 1 ] = obj_p ->
NameInternal -> size [ 1 ] ; e3n3mnwk4b ( jtype , b_kstr ) ; if ( jtype ->
size [ 1 ] != 0 ) { b_kstr = jtype -> size [ 0 ] * jtype -> size [ 1 ] ;
jtype -> size [ 0 ] = 1 ; jtype -> size [ 1 ] = obj_p -> NameInternal -> size
[ 1 ] ; e3n3mnwk4b ( jtype , b_kstr ) ; minnanb = obj_p -> NameInternal ->
size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < minnanb ; b_kstr ++ ) { jtype ->
data [ b_kstr ] = obj_p -> NameInternal -> data [ b_kstr ] ; } if ( ! iobj_1
[ 1 ] . InTree ) { b_kstr = iobj_1 [ 1 ] . NameInternal -> size [ 0 ] *
iobj_1 [ 1 ] . NameInternal -> size [ 1 ] ; iobj_1 [ 1 ] . NameInternal ->
size [ 0 ] = 1 ; iobj_1 [ 1 ] . NameInternal -> size [ 1 ] = jtype -> size [
1 ] ; e3n3mnwk4b ( iobj_1 [ 1 ] . NameInternal , b_kstr ) ; minnanb = jtype
-> size [ 1 ] - 1 ; for ( b_kstr = 0 ; b_kstr <= minnanb ; b_kstr ++ ) {
iobj_1 [ 1 ] . NameInternal -> data [ b_kstr ] = jtype -> data [ b_kstr ] ; }
} } fagcrr3avn ( & jtype ) ; b_k = obj_p -> PositionLimitsInternal -> size [
0 ] << 1 ; b_kstr = iobj_1 [ 1 ] . PositionLimitsInternal -> size [ 0 ] *
iobj_1 [ 1 ] . PositionLimitsInternal -> size [ 1 ] ; iobj_1 [ 1 ] .
PositionLimitsInternal -> size [ 0 ] = obj_p -> PositionLimitsInternal ->
size [ 0 ] ; iobj_1 [ 1 ] . PositionLimitsInternal -> size [ 1 ] = 2 ;
miyl1rq4uj ( iobj_1 [ 1 ] . PositionLimitsInternal , b_kstr ) ; calykgtvmm (
& obj_i , 1 ) ; b_kstr = obj_i -> size [ 0 ] ; obj_i -> size [ 0 ] = b_k ;
miyl1rq4uj ( obj_i , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < b_k ; b_kstr ++ )
{ obj_i -> data [ b_kstr ] = obj_p -> PositionLimitsInternal -> data [ b_kstr
] ; } minnanb = obj_i -> size [ 0 ] ; for ( b_kstr = 0 ; b_kstr < minnanb ;
b_kstr ++ ) { iobj_1 [ 1 ] . PositionLimitsInternal -> data [ b_kstr ] =
obj_i -> data [ b_kstr ] ; } b_kstr = obj_i -> size [ 0 ] ; obj_i -> size [ 0
] = obj_p -> HomePositionInternal -> size [ 0 ] ; miyl1rq4uj ( obj_i , b_kstr
) ; minnanb = obj_p -> HomePositionInternal -> size [ 0 ] ; for ( b_kstr = 0
; b_kstr < minnanb ; b_kstr ++ ) { obj_i -> data [ b_kstr ] = obj_p ->
HomePositionInternal -> data [ b_kstr ] ; } b_kstr = iobj_1 [ 1 ] .
HomePositionInternal -> size [ 0 ] ; iobj_1 [ 1 ] . HomePositionInternal ->
size [ 0 ] = obj_i -> size [ 0 ] ; miyl1rq4uj ( iobj_1 [ 1 ] .
HomePositionInternal , b_kstr ) ; minnanb = obj_i -> size [ 0 ] ; for (
b_kstr = 0 ; b_kstr < minnanb ; b_kstr ++ ) { iobj_1 [ 1 ] .
HomePositionInternal -> data [ b_kstr ] = obj_i -> data [ b_kstr ] ; } b_x =
obj_p -> JointAxisInternal [ 0 ] ; obj_idx_1 = obj_p -> JointAxisInternal [ 1
] ; obj_idx_2 = obj_p -> JointAxisInternal [ 2 ] ; iobj_1 [ 1 ] .
JointAxisInternal [ 0 ] = b_x ; iobj_1 [ 1 ] . JointAxisInternal [ 1 ] =
obj_idx_1 ; iobj_1 [ 1 ] . JointAxisInternal [ 2 ] = obj_idx_2 ; b_k = 6 *
obj_p -> MotionSubspace -> size [ 1 ] ; b_kstr = iobj_1 [ 1 ] .
MotionSubspace -> size [ 0 ] * iobj_1 [ 1 ] . MotionSubspace -> size [ 1 ] ;
iobj_1 [ 1 ] . MotionSubspace -> size [ 0 ] = 6 ; iobj_1 [ 1 ] .
MotionSubspace -> size [ 1 ] = obj_p -> MotionSubspace -> size [ 1 ] ;
miyl1rq4uj ( iobj_1 [ 1 ] . MotionSubspace , b_kstr ) ; b_kstr = obj_i ->
size [ 0 ] ; obj_i -> size [ 0 ] = b_k ; miyl1rq4uj ( obj_i , b_kstr ) ; for
( b_kstr = 0 ; b_kstr < b_k ; b_kstr ++ ) { obj_i -> data [ b_kstr ] = obj_p
-> MotionSubspace -> data [ b_kstr ] ; } minnanb = obj_i -> size [ 0 ] ; for
( b_kstr = 0 ; b_kstr < minnanb ; b_kstr ++ ) { iobj_1 [ 1 ] . MotionSubspace
-> data [ b_kstr ] = obj_i -> data [ b_kstr ] ; } bdhoypqe5x ( & obj_i ) ;
for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj_m [ b_kstr ] = obj_p ->
JointToParentTransform [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr
++ ) { iobj_1 [ 1 ] . JointToParentTransform [ b_kstr ] = obj_m [ b_kstr ] ;
} for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj_m [ b_kstr ] = obj_p ->
ChildToJointTransform [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr
++ ) { iobj_1 [ 1 ] . ChildToJointTransform [ b_kstr ] = obj_m [ b_kstr ] ; }
iobj_2 -> JointInternal = & iobj_1 [ 1 ] ; iobj_2 -> MassInternal = obj ->
MassInternal ; b_x = obj -> CenterOfMassInternal [ 0 ] ; obj_idx_1 = obj ->
CenterOfMassInternal [ 1 ] ; obj_idx_2 = obj -> CenterOfMassInternal [ 2 ] ;
iobj_2 -> CenterOfMassInternal [ 0 ] = b_x ; iobj_2 -> CenterOfMassInternal [
1 ] = obj_idx_1 ; iobj_2 -> CenterOfMassInternal [ 2 ] = obj_idx_2 ; for (
b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { obj_g [ b_kstr ] = obj ->
InertiaInternal [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) {
iobj_2 -> InertiaInternal [ b_kstr ] = obj_g [ b_kstr ] ; } for ( b_kstr = 0
; b_kstr < 36 ; b_kstr ++ ) { obj_j [ b_kstr ] = obj -> SpatialInertia [
b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) { iobj_2 ->
SpatialInertia [ b_kstr ] = obj_j [ b_kstr ] ; } obj_e = obj ->
CollisionsInternal ; newObj = p1kacjwirim ( & iobj_0 [ 1 ] , obj_e ->
MaxElements ) ; newObj -> Size = obj_e -> Size ; b_x = obj_e -> Size ;
minnanb = ( int32_T ) b_x - 1 ; for ( b_k = 0 ; b_k <= minnanb ; b_k ++ ) {
obj_CollisionPrimitive = obj_e -> CollisionGeometries -> data [ b_k ] .
CollisionPrimitive ; for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj_m [
b_kstr ] = obj_e -> CollisionGeometries -> data [ b_k ] . LocalPose [ b_kstr
] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj_WorldPose [ b_kstr ]
= obj_e -> CollisionGeometries -> data [ b_k ] . WorldPose [ b_kstr ] ; }
obj_CollisionPrimitive = collisioncodegen_copyGeometry (
obj_CollisionPrimitive ) ; newObj -> CollisionGeometries -> data [ b_k ] .
CollisionPrimitive = obj_CollisionPrimitive ; for ( b_kstr = 0 ; b_kstr < 16
; b_kstr ++ ) { newObj -> CollisionGeometries -> data [ b_k ] . LocalPose [
b_kstr ] = obj_m [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ )
{ newObj -> CollisionGeometries -> data [ b_k ] . WorldPose [ b_kstr ] =
obj_WorldPose [ b_kstr ] ; } } iobj_2 -> CollisionsInternal = newObj ; return
newbody ; } static void jem4veejvh ( m0caqfktx0 * obj , alyuhn31fbfz * bodyin
, const e1n154mvbh * parentName , ogx4llc4arl * iobj_0 , ot4uovzkt0w * iobj_1
, alyuhn31fbfz * iobj_2 ) { alyuhn31fbfz * body ; e1n154mvbh * bname ;
ot4uovzkt0w * jnt ; real_T b_index ; real_T pid ; int32_T b_kstr ; int32_T
loop_ub ; char_T b [ 5 ] ; boolean_T b_bool ; static const char_T tmp [ 5 ] =
{ 'f' , 'i' , 'x' , 'e' , 'd' } ; int32_T exitg1 ; m2n1niwoq4 ( & bname , 2 )
; b_kstr = bname -> size [ 0 ] * bname -> size [ 1 ] ; bname -> size [ 0 ] =
1 ; bname -> size [ 1 ] = bodyin -> NameInternal -> size [ 1 ] ; e3n3mnwk4b (
bname , b_kstr ) ; loop_ub = bodyin -> NameInternal -> size [ 1 ] ; for (
b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) { bname -> data [ b_kstr ] =
bodyin -> NameInternal -> data [ b_kstr ] ; } jjyjgxfdua ( obj , bname ) ;
pid = jjyjgxfdua ( obj , parentName ) ; b_index = obj -> NumBodies + 1.0 ;
body = pfo1apkwc0 ( bodyin , & iobj_0 [ 0 ] , & iobj_1 [ 0 ] , iobj_2 ) ; obj
-> Bodies [ ( int32_T ) b_index - 1 ] = body ; body -> Index = b_index ; body
-> ParentIndex = pid ; body -> JointInternal -> InTree = true ; obj ->
NumBodies ++ ; jnt = body -> JointInternal ; b_kstr = bname -> size [ 0 ] *
bname -> size [ 1 ] ; bname -> size [ 0 ] = 1 ; bname -> size [ 1 ] = jnt ->
Type -> size [ 1 ] ; e3n3mnwk4b ( bname , b_kstr ) ; loop_ub = jnt -> Type ->
size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) { bname ->
data [ b_kstr ] = jnt -> Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ;
b_kstr < 5 ; b_kstr ++ ) { b [ b_kstr ] = tmp [ b_kstr ] ; } b_bool = false ;
if ( bname -> size [ 1 ] != 5 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if
( b_kstr - 1 < 5 ) { if ( bname -> data [ b_kstr - 1 ] != b [ b_kstr - 1 ] )
{ exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; }
} while ( exitg1 == 0 ) ; } fagcrr3avn ( & bname ) ; if ( ! b_bool ) { obj ->
NumNonFixedBodies ++ ; jnt = body -> JointInternal ; b_kstr = ( int32_T )
body -> Index - 1 ; obj -> PositionDoFMap [ b_kstr ] = obj -> PositionNumber
+ 1.0 ; obj -> PositionDoFMap [ b_kstr + 9 ] = obj -> PositionNumber + jnt ->
PositionNumber ; jnt = body -> JointInternal ; b_kstr = ( int32_T ) body ->
Index - 1 ; obj -> VelocityDoFMap [ b_kstr ] = obj -> VelocityNumber + 1.0 ;
obj -> VelocityDoFMap [ b_kstr + 9 ] = obj -> VelocityNumber + jnt ->
VelocityNumber ; } else { b_kstr = ( int32_T ) body -> Index - 1 ; obj ->
PositionDoFMap [ b_kstr ] = 0.0 ; obj -> PositionDoFMap [ b_kstr + 9 ] = -
1.0 ; b_kstr = ( int32_T ) body -> Index - 1 ; obj -> VelocityDoFMap [ b_kstr
] = 0.0 ; obj -> VelocityDoFMap [ b_kstr + 9 ] = - 1.0 ; } jnt = body ->
JointInternal ; obj -> PositionNumber += jnt -> PositionNumber ; jnt = body
-> JointInternal ; obj -> VelocityNumber += jnt -> VelocityNumber ; } static
void omjnvaudda ( carpjyrt23 * obj , mjv2tzlyyigm * rigidbodytree ,
ot4uovzkt0w * iobj_0 , alyuhn31fbfz * iobj_1 , ogx4llc4arl * iobj_2 ,
m0caqfktx0 * iobj_3 ) { void * obj_CollisionPrimitive ; alyuhn31fbfz * body ;
alyuhn31fbfz * parent ; e1n154mvbh * bname ; e1n154mvbh * switch_expression ;
ogx4llc4arl * iobj_0_p ; ogx4llc4arl * newObj ; ot4uovzkt0w * iobj_1_p ;
real_T obj_LocalPose [ 16 ] ; real_T obj_WorldPose [ 16 ] ; real_T
poslim_data [ 12 ] ; real_T b ; real_T bid ; int32_T b_kstr ; int32_T loop_ub
; char_T b_e [ 9 ] ; char_T b_p [ 8 ] ; int8_T msubspace_data [ 36 ] ; int8_T
b_I [ 9 ] ; int8_T tmp [ 6 ] ; boolean_T b_bool ; static const int8_T tmp_p [
16 ] = { 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 } ;
static const char_T tmp_e [ 8 ] = { 'b' , 'a' , 's' , 'e' , '_' , 'j' , 'n' ,
't' } ; static const char_T tmp_i [ 5 ] = { 'f' , 'i' , 'x' , 'e' , 'd' } ;
static const char_T tmp_m [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't' ,
'e' } ; static const char_T tmp_g [ 9 ] = { 'p' , 'r' , 'i' , 's' , 'm' , 'a'
, 't' , 'i' , 'c' } ; real_T unusedExpr [ 5 ] ; int32_T exitg1 ; int32_T i ;
boolean_T exitg2 ; body = & iobj_3 -> Base ; iobj_0_p = & iobj_3 -> _pobj1 [
0 ] ; i = body -> NameInternal -> size [ 0 ] * body -> NameInternal -> size [
1 ] ; body -> NameInternal -> size [ 0 ] = 1 ; body -> NameInternal -> size [
1 ] = 4 ; e3n3mnwk4b ( body -> NameInternal , i ) ; body -> NameInternal ->
data [ 0 ] = 'b' ; body -> NameInternal -> data [ 1 ] = 'a' ; body ->
NameInternal -> data [ 2 ] = 's' ; body -> NameInternal -> data [ 3 ] = 'e' ;
iobj_3 -> _pobj2 [ 0 ] . InTree = false ; for ( i = 0 ; i < 16 ; i ++ ) {
iobj_3 -> _pobj2 [ 0 ] . JointToParentTransform [ i ] = tmp_p [ i ] ; } for (
i = 0 ; i < 16 ; i ++ ) { iobj_3 -> _pobj2 [ 0 ] . ChildToJointTransform [ i
] = tmp_p [ i ] ; } i = iobj_3 -> _pobj2 [ 0 ] . NameInternal -> size [ 0 ] *
iobj_3 -> _pobj2 [ 0 ] . NameInternal -> size [ 1 ] ; iobj_3 -> _pobj2 [ 0 ]
. NameInternal -> size [ 0 ] = 1 ; iobj_3 -> _pobj2 [ 0 ] . NameInternal ->
size [ 1 ] = 8 ; e3n3mnwk4b ( iobj_3 -> _pobj2 [ 0 ] . NameInternal , i ) ;
for ( i = 0 ; i < 8 ; i ++ ) { iobj_3 -> _pobj2 [ 0 ] . NameInternal -> data
[ i ] = tmp_e [ i ] ; } i = iobj_3 -> _pobj2 [ 0 ] . Type -> size [ 0 ] *
iobj_3 -> _pobj2 [ 0 ] . Type -> size [ 1 ] ; iobj_3 -> _pobj2 [ 0 ] . Type
-> size [ 0 ] = 1 ; iobj_3 -> _pobj2 [ 0 ] . Type -> size [ 1 ] = 5 ;
e3n3mnwk4b ( iobj_3 -> _pobj2 [ 0 ] . Type , i ) ; for ( i = 0 ; i < 5 ; i ++
) { iobj_3 -> _pobj2 [ 0 ] . Type -> data [ i ] = tmp_i [ i ] ; } m2n1niwoq4
( & switch_expression , 2 ) ; i = switch_expression -> size [ 0 ] *
switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = iobj_3 -> _pobj2 [ 0 ] . Type -> size [ 1 ]
; e3n3mnwk4b ( switch_expression , i ) ; loop_ub = iobj_3 -> _pobj2 [ 0 ] .
Type -> size [ 1 ] ; for ( i = 0 ; i < loop_ub ; i ++ ) { switch_expression
-> data [ i ] = iobj_3 -> _pobj2 [ 0 ] . Type -> data [ i ] ; } for ( i = 0 ;
i < 8 ; i ++ ) { b_p [ i ] = tmp_m [ i ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { bid = 0.0 ;
} else { for ( i = 0 ; i < 9 ; i ++ ) { b_e [ i ] = tmp_g [ i ] ; } if (
switch_expression -> size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b_e [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { bid = 1.0 ;
} else { bid = - 1.0 ; } } switch ( ( int32_T ) bid ) { case 0 : tmp [ 0 ] =
0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ]
= 0 ; for ( i = 0 ; i < 6 ; i ++ ) { msubspace_data [ i ] = tmp [ i ] ; }
poslim_data [ 0 ] = - 3.1415926535897931 ; poslim_data [ 1 ] =
3.1415926535897931 ; iobj_3 -> _pobj2 [ 0 ] . VelocityNumber = 1.0 ; iobj_3
-> _pobj2 [ 0 ] . PositionNumber = 1.0 ; iobj_3 -> _pobj2 [ 0 ] .
JointAxisInternal [ 0 ] = 0.0 ; iobj_3 -> _pobj2 [ 0 ] . JointAxisInternal [
1 ] = 0.0 ; iobj_3 -> _pobj2 [ 0 ] . JointAxisInternal [ 2 ] = 1.0 ; break ;
case 1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] = 0 ; tmp
[ 4 ] = 0 ; tmp [ 5 ] = 1 ; for ( i = 0 ; i < 6 ; i ++ ) { msubspace_data [ i
] = tmp [ i ] ; } poslim_data [ 0 ] = - 0.5 ; poslim_data [ 1 ] = 0.5 ;
iobj_3 -> _pobj2 [ 0 ] . VelocityNumber = 1.0 ; iobj_3 -> _pobj2 [ 0 ] .
PositionNumber = 1.0 ; iobj_3 -> _pobj2 [ 0 ] . JointAxisInternal [ 0 ] = 0.0
; iobj_3 -> _pobj2 [ 0 ] . JointAxisInternal [ 1 ] = 0.0 ; iobj_3 -> _pobj2 [
0 ] . JointAxisInternal [ 2 ] = 1.0 ; break ; default : for ( i = 0 ; i < 6 ;
i ++ ) { msubspace_data [ i ] = 0 ; } poslim_data [ 0 ] = 0.0 ; poslim_data [
1 ] = 0.0 ; iobj_3 -> _pobj2 [ 0 ] . VelocityNumber = 0.0 ; iobj_3 -> _pobj2
[ 0 ] . PositionNumber = 0.0 ; iobj_3 -> _pobj2 [ 0 ] . JointAxisInternal [ 0
] = 0.0 ; iobj_3 -> _pobj2 [ 0 ] . JointAxisInternal [ 1 ] = 0.0 ; iobj_3 ->
_pobj2 [ 0 ] . JointAxisInternal [ 2 ] = 0.0 ; break ; } i = iobj_3 -> _pobj2
[ 0 ] . MotionSubspace -> size [ 0 ] * iobj_3 -> _pobj2 [ 0 ] .
MotionSubspace -> size [ 1 ] ; iobj_3 -> _pobj2 [ 0 ] . MotionSubspace ->
size [ 0 ] = 6 ; iobj_3 -> _pobj2 [ 0 ] . MotionSubspace -> size [ 1 ] = 1 ;
miyl1rq4uj ( iobj_3 -> _pobj2 [ 0 ] . MotionSubspace , i ) ; for ( i = 0 ; i
< 6 ; i ++ ) { iobj_3 -> _pobj2 [ 0 ] . MotionSubspace -> data [ i ] =
msubspace_data [ i ] ; } i = iobj_3 -> _pobj2 [ 0 ] . PositionLimitsInternal
-> size [ 0 ] * iobj_3 -> _pobj2 [ 0 ] . PositionLimitsInternal -> size [ 1 ]
; iobj_3 -> _pobj2 [ 0 ] . PositionLimitsInternal -> size [ 0 ] = 1 ; iobj_3
-> _pobj2 [ 0 ] . PositionLimitsInternal -> size [ 1 ] = 2 ; miyl1rq4uj (
iobj_3 -> _pobj2 [ 0 ] . PositionLimitsInternal , i ) ; for ( i = 0 ; i < 2 ;
i ++ ) { iobj_3 -> _pobj2 [ 0 ] . PositionLimitsInternal -> data [ i ] =
poslim_data [ i ] ; } i = iobj_3 -> _pobj2 [ 0 ] . HomePositionInternal ->
size [ 0 ] ; iobj_3 -> _pobj2 [ 0 ] . HomePositionInternal -> size [ 0 ] = 1
; miyl1rq4uj ( iobj_3 -> _pobj2 [ 0 ] . HomePositionInternal , i ) ; iobj_3
-> _pobj2 [ 0 ] . HomePositionInternal -> data [ 0 ] = 0.0 ; body ->
JointInternal = & iobj_3 -> _pobj2 [ 0 ] ; body -> Index = - 1.0 ; body ->
ParentIndex = - 1.0 ; body -> MassInternal = 1.0 ; body ->
CenterOfMassInternal [ 0 ] = 0.0 ; body -> CenterOfMassInternal [ 1 ] = 0.0 ;
body -> CenterOfMassInternal [ 2 ] = 0.0 ; for ( i = 0 ; i < 9 ; i ++ ) { b_I
[ i ] = 0 ; } b_I [ 0 ] = 1 ; b_I [ 4 ] = 1 ; b_I [ 8 ] = 1 ; for ( i = 0 ; i
< 9 ; i ++ ) { body -> InertiaInternal [ i ] = b_I [ i ] ; } for ( i = 0 ; i
< 36 ; i ++ ) { msubspace_data [ i ] = 0 ; } for ( b_kstr = 0 ; b_kstr < 6 ;
b_kstr ++ ) { msubspace_data [ b_kstr + 6 * b_kstr ] = 1 ; } for ( i = 0 ; i
< 36 ; i ++ ) { body -> SpatialInertia [ i ] = msubspace_data [ i ] ; } body
-> CollisionsInternal = p1kacjwirim ( iobj_0_p , 0.0 ) ; body ->
matlabCodegenIsDeleted = false ; iobj_3 -> Base . Index = 0.0 ; jlodq4k5z5 (
unusedExpr ) ; iobj_0_p = & iobj_3 -> _pobj1 [ 1 ] ; iobj_1_p = & iobj_3 ->
_pobj2 [ 1 ] ; body = & iobj_3 -> _pobj0 [ 0 ] ; iobj_3 -> Bodies [ 0 ] =
lwkmqybjwb ( & ( & ( & iobj_3 -> _pobj0 [ 0 ] ) [ 0 ] ) [ 0 ] , & ( &
iobj_0_p [ 0 ] ) [ 0 ] , & ( & iobj_1_p [ 0 ] ) [ 0 ] ) ; iobj_3 -> Bodies [
1 ] = e0kx4oauic ( & ( & body [ 0 ] ) [ 1 ] , & ( & iobj_0_p [ 0 ] ) [ 1 ] ,
& ( & iobj_1_p [ 0 ] ) [ 1 ] ) ; iobj_3 -> Bodies [ 2 ] = mpzyhljwzq ( & ( &
body [ 0 ] ) [ 2 ] , & ( & iobj_0_p [ 0 ] ) [ 2 ] , & ( & iobj_1_p [ 0 ] ) [
2 ] ) ; iobj_3 -> Bodies [ 3 ] = axww4prr0o ( & ( & body [ 0 ] ) [ 3 ] , & (
& iobj_0_p [ 0 ] ) [ 3 ] , & ( & iobj_1_p [ 0 ] ) [ 3 ] ) ; iobj_3 -> Bodies
[ 4 ] = oh2xpcdbml ( & ( & body [ 0 ] ) [ 4 ] , & ( & iobj_0_p [ 0 ] ) [ 4 ]
, & ( & iobj_1_p [ 0 ] ) [ 4 ] ) ; iobj_3 -> Bodies [ 5 ] = hcmkep3fjc ( & (
& body [ 0 ] ) [ 5 ] , & ( & iobj_0_p [ 0 ] ) [ 5 ] , & ( & iobj_1_p [ 0 ] )
[ 5 ] ) ; iobj_3 -> Bodies [ 6 ] = eearo4zkrq ( & ( & body [ 0 ] ) [ 6 ] , &
( & iobj_0_p [ 0 ] ) [ 6 ] , & ( & iobj_1_p [ 0 ] ) [ 6 ] ) ; iobj_3 ->
Bodies [ 7 ] = et05vlhztn ( & ( & body [ 0 ] ) [ 7 ] , & ( & iobj_0_p [ 0 ] )
[ 7 ] , & ( & iobj_1_p [ 0 ] ) [ 7 ] ) ; iobj_3 -> Bodies [ 8 ] = oct3agbiik
( & ( & body [ 0 ] ) [ 8 ] , & ( & iobj_0_p [ 0 ] ) [ 8 ] , & ( & iobj_1_p [
0 ] ) [ 8 ] ) ; iobj_3 -> NumBodies = 0.0 ; iobj_3 -> NumNonFixedBodies = 0.0
; iobj_3 -> PositionNumber = 0.0 ; iobj_3 -> VelocityNumber = 0.0 ;
jlodq4k5z5 ( unusedExpr ) ; for ( i = 0 ; i < 9 ; i ++ ) { iobj_3 ->
PositionDoFMap [ i ] = 0.0 ; } for ( i = 0 ; i < 9 ; i ++ ) { iobj_3 ->
PositionDoFMap [ i + 9 ] = - 1.0 ; } for ( i = 0 ; i < 9 ; i ++ ) { iobj_3 ->
VelocityDoFMap [ i ] = 0.0 ; } for ( i = 0 ; i < 9 ; i ++ ) { iobj_3 ->
VelocityDoFMap [ i + 9 ] = - 1.0 ; } iobj_3 -> matlabCodegenIsDeleted = false
; i = switch_expression -> size [ 0 ] * switch_expression -> size [ 1 ] ;
switch_expression -> size [ 0 ] = 1 ; switch_expression -> size [ 1 ] =
rigidbodytree -> Base . NameInternal -> size [ 1 ] ; e3n3mnwk4b (
switch_expression , i ) ; loop_ub = rigidbodytree -> Base . NameInternal ->
size [ 1 ] ; for ( i = 0 ; i < loop_ub ; i ++ ) { switch_expression -> data [
i ] = rigidbodytree -> Base . NameInternal -> data [ i ] ; } bid = - 1.0 ;
m2n1niwoq4 ( & bname , 2 ) ; i = bname -> size [ 0 ] * bname -> size [ 1 ] ;
bname -> size [ 0 ] = 1 ; bname -> size [ 1 ] = iobj_3 -> Base . NameInternal
-> size [ 1 ] ; e3n3mnwk4b ( bname , i ) ; loop_ub = iobj_3 -> Base .
NameInternal -> size [ 1 ] ; for ( i = 0 ; i < loop_ub ; i ++ ) { bname ->
data [ i ] = iobj_3 -> Base . NameInternal -> data [ i ] ; } if ( e3fq41hhh0
( bname , switch_expression ) ) { bid = 0.0 ; } else { b = iobj_3 ->
NumBodies ; b_kstr = 0 ; exitg2 = false ; while ( ( ! exitg2 ) && ( b_kstr <=
( int32_T ) b - 1 ) ) { body = iobj_3 -> Bodies [ b_kstr ] ; i = bname ->
size [ 0 ] * bname -> size [ 1 ] ; bname -> size [ 0 ] = 1 ; bname -> size [
1 ] = body -> NameInternal -> size [ 1 ] ; e3n3mnwk4b ( bname , i ) ; loop_ub
= body -> NameInternal -> size [ 1 ] ; for ( i = 0 ; i < loop_ub ; i ++ ) {
bname -> data [ i ] = body -> NameInternal -> data [ i ] ; } if ( e3fq41hhh0
( bname , switch_expression ) ) { bid = ( real_T ) b_kstr + 1.0 ; exitg2 =
true ; } else { b_kstr ++ ; } } } if ( ( ! ( bid == 0.0 ) ) && ( bid < 0.0 )
) { i = iobj_3 -> Base . NameInternal -> size [ 0 ] * iobj_3 -> Base .
NameInternal -> size [ 1 ] ; iobj_3 -> Base . NameInternal -> size [ 0 ] = 1
; iobj_3 -> Base . NameInternal -> size [ 1 ] = switch_expression -> size [ 1
] ; e3n3mnwk4b ( iobj_3 -> Base . NameInternal , i ) ; loop_ub =
switch_expression -> size [ 1 ] - 1 ; for ( i = 0 ; i <= loop_ub ; i ++ ) {
iobj_3 -> Base . NameInternal -> data [ i ] = switch_expression -> data [ i ]
; } } fagcrr3avn ( & switch_expression ) ; iobj_0_p = rigidbodytree -> Base .
CollisionsInternal ; newObj = p1kacjwirim ( & ( & iobj_2 [ 0 ] ) [ 0 ] ,
iobj_0_p -> MaxElements ) ; newObj -> Size = iobj_0_p -> Size ; b = iobj_0_p
-> Size ; loop_ub = ( int32_T ) b - 1 ; for ( b_kstr = 0 ; b_kstr <= loop_ub
; b_kstr ++ ) { obj_CollisionPrimitive = iobj_0_p -> CollisionGeometries ->
data [ b_kstr ] . CollisionPrimitive ; for ( i = 0 ; i < 16 ; i ++ ) {
obj_LocalPose [ i ] = iobj_0_p -> CollisionGeometries -> data [ b_kstr ] .
LocalPose [ i ] ; } for ( i = 0 ; i < 16 ; i ++ ) { obj_WorldPose [ i ] =
iobj_0_p -> CollisionGeometries -> data [ b_kstr ] . WorldPose [ i ] ; }
obj_CollisionPrimitive = collisioncodegen_copyGeometry (
obj_CollisionPrimitive ) ; newObj -> CollisionGeometries -> data [ b_kstr ] .
CollisionPrimitive = obj_CollisionPrimitive ; for ( i = 0 ; i < 16 ; i ++ ) {
newObj -> CollisionGeometries -> data [ b_kstr ] . LocalPose [ i ] =
obj_LocalPose [ i ] ; } for ( i = 0 ; i < 16 ; i ++ ) { newObj ->
CollisionGeometries -> data [ b_kstr ] . WorldPose [ i ] = obj_WorldPose [ i
] ; } } iobj_3 -> Base . CollisionsInternal = newObj ; if ( rigidbodytree ->
NumBodies >= 1.0 ) { body = rigidbodytree -> Bodies [ 0 ] ; bid = body ->
ParentIndex ; if ( bid > 0.0 ) { parent = rigidbodytree -> Bodies [ ( int32_T
) bid - 1 ] ; } else { parent = & rigidbodytree -> Base ; } i = bname -> size
[ 0 ] * bname -> size [ 1 ] ; bname -> size [ 0 ] = 1 ; bname -> size [ 1 ] =
parent -> NameInternal -> size [ 1 ] ; e3n3mnwk4b ( bname , i ) ; loop_ub =
parent -> NameInternal -> size [ 1 ] ; for ( i = 0 ; i < loop_ub ; i ++ ) {
bname -> data [ i ] = parent -> NameInternal -> data [ i ] ; } jem4veejvh (
iobj_3 , body , bname , & ( & iobj_2 [ 0 ] ) [ 1 ] , & ( & iobj_0 [ 0 ] ) [ 0
] , & ( & iobj_1 [ 0 ] ) [ 0 ] ) ; } if ( rigidbodytree -> NumBodies >= 2.0 )
{ body = rigidbodytree -> Bodies [ 1 ] ; bid = body -> ParentIndex ; if ( bid
> 0.0 ) { parent = rigidbodytree -> Bodies [ ( int32_T ) bid - 1 ] ; } else {
parent = & rigidbodytree -> Base ; } i = bname -> size [ 0 ] * bname -> size
[ 1 ] ; bname -> size [ 0 ] = 1 ; bname -> size [ 1 ] = parent ->
NameInternal -> size [ 1 ] ; e3n3mnwk4b ( bname , i ) ; loop_ub = parent ->
NameInternal -> size [ 1 ] ; for ( i = 0 ; i < loop_ub ; i ++ ) { bname ->
data [ i ] = parent -> NameInternal -> data [ i ] ; } jem4veejvh ( iobj_3 ,
body , bname , & ( & iobj_2 [ 0 ] ) [ 3 ] , & ( & iobj_0 [ 0 ] ) [ 2 ] , & (
& iobj_1 [ 0 ] ) [ 1 ] ) ; } if ( rigidbodytree -> NumBodies >= 3.0 ) { body
= rigidbodytree -> Bodies [ 2 ] ; bid = body -> ParentIndex ; if ( bid > 0.0
) { parent = rigidbodytree -> Bodies [ ( int32_T ) bid - 1 ] ; } else {
parent = & rigidbodytree -> Base ; } i = bname -> size [ 0 ] * bname -> size
[ 1 ] ; bname -> size [ 0 ] = 1 ; bname -> size [ 1 ] = parent ->
NameInternal -> size [ 1 ] ; e3n3mnwk4b ( bname , i ) ; loop_ub = parent ->
NameInternal -> size [ 1 ] ; for ( i = 0 ; i < loop_ub ; i ++ ) { bname ->
data [ i ] = parent -> NameInternal -> data [ i ] ; } jem4veejvh ( iobj_3 ,
body , bname , & ( & iobj_2 [ 0 ] ) [ 5 ] , & ( & iobj_0 [ 0 ] ) [ 4 ] , & (
& iobj_1 [ 0 ] ) [ 2 ] ) ; } if ( rigidbodytree -> NumBodies >= 4.0 ) { body
= rigidbodytree -> Bodies [ 3 ] ; bid = body -> ParentIndex ; if ( bid > 0.0
) { parent = rigidbodytree -> Bodies [ ( int32_T ) bid - 1 ] ; } else {
parent = & rigidbodytree -> Base ; } i = bname -> size [ 0 ] * bname -> size
[ 1 ] ; bname -> size [ 0 ] = 1 ; bname -> size [ 1 ] = parent ->
NameInternal -> size [ 1 ] ; e3n3mnwk4b ( bname , i ) ; loop_ub = parent ->
NameInternal -> size [ 1 ] ; for ( i = 0 ; i < loop_ub ; i ++ ) { bname ->
data [ i ] = parent -> NameInternal -> data [ i ] ; } jem4veejvh ( iobj_3 ,
body , bname , & ( & iobj_2 [ 0 ] ) [ 7 ] , & ( & iobj_0 [ 0 ] ) [ 6 ] , & (
& iobj_1 [ 0 ] ) [ 3 ] ) ; } if ( rigidbodytree -> NumBodies >= 5.0 ) { body
= rigidbodytree -> Bodies [ 4 ] ; bid = body -> ParentIndex ; if ( bid > 0.0
) { parent = rigidbodytree -> Bodies [ ( int32_T ) bid - 1 ] ; } else {
parent = & rigidbodytree -> Base ; } i = bname -> size [ 0 ] * bname -> size
[ 1 ] ; bname -> size [ 0 ] = 1 ; bname -> size [ 1 ] = parent ->
NameInternal -> size [ 1 ] ; e3n3mnwk4b ( bname , i ) ; loop_ub = parent ->
NameInternal -> size [ 1 ] ; for ( i = 0 ; i < loop_ub ; i ++ ) { bname ->
data [ i ] = parent -> NameInternal -> data [ i ] ; } jem4veejvh ( iobj_3 ,
body , bname , & ( & iobj_2 [ 0 ] ) [ 9 ] , & ( & iobj_0 [ 0 ] ) [ 8 ] , & (
& iobj_1 [ 0 ] ) [ 4 ] ) ; } if ( rigidbodytree -> NumBodies >= 6.0 ) { body
= rigidbodytree -> Bodies [ 5 ] ; bid = body -> ParentIndex ; if ( bid > 0.0
) { parent = rigidbodytree -> Bodies [ ( int32_T ) bid - 1 ] ; } else {
parent = & rigidbodytree -> Base ; } i = bname -> size [ 0 ] * bname -> size
[ 1 ] ; bname -> size [ 0 ] = 1 ; bname -> size [ 1 ] = parent ->
NameInternal -> size [ 1 ] ; e3n3mnwk4b ( bname , i ) ; loop_ub = parent ->
NameInternal -> size [ 1 ] ; for ( i = 0 ; i < loop_ub ; i ++ ) { bname ->
data [ i ] = parent -> NameInternal -> data [ i ] ; } jem4veejvh ( iobj_3 ,
body , bname , & ( & iobj_2 [ 0 ] ) [ 11 ] , & ( & iobj_0 [ 0 ] ) [ 10 ] , &
( & iobj_1 [ 0 ] ) [ 5 ] ) ; } if ( rigidbodytree -> NumBodies >= 7.0 ) {
body = rigidbodytree -> Bodies [ 6 ] ; bid = body -> ParentIndex ; if ( bid >
0.0 ) { parent = rigidbodytree -> Bodies [ ( int32_T ) bid - 1 ] ; } else {
parent = & rigidbodytree -> Base ; } i = bname -> size [ 0 ] * bname -> size
[ 1 ] ; bname -> size [ 0 ] = 1 ; bname -> size [ 1 ] = parent ->
NameInternal -> size [ 1 ] ; e3n3mnwk4b ( bname , i ) ; loop_ub = parent ->
NameInternal -> size [ 1 ] ; for ( i = 0 ; i < loop_ub ; i ++ ) { bname ->
data [ i ] = parent -> NameInternal -> data [ i ] ; } jem4veejvh ( iobj_3 ,
body , bname , & ( & iobj_2 [ 0 ] ) [ 13 ] , & ( & iobj_0 [ 0 ] ) [ 12 ] , &
( & iobj_1 [ 0 ] ) [ 6 ] ) ; } if ( rigidbodytree -> NumBodies >= 8.0 ) {
body = rigidbodytree -> Bodies [ 7 ] ; bid = body -> ParentIndex ; if ( bid >
0.0 ) { parent = rigidbodytree -> Bodies [ ( int32_T ) bid - 1 ] ; } else {
parent = & rigidbodytree -> Base ; } i = bname -> size [ 0 ] * bname -> size
[ 1 ] ; bname -> size [ 0 ] = 1 ; bname -> size [ 1 ] = parent ->
NameInternal -> size [ 1 ] ; e3n3mnwk4b ( bname , i ) ; loop_ub = parent ->
NameInternal -> size [ 1 ] ; for ( i = 0 ; i < loop_ub ; i ++ ) { bname ->
data [ i ] = parent -> NameInternal -> data [ i ] ; } jem4veejvh ( iobj_3 ,
body , bname , & ( & iobj_2 [ 0 ] ) [ 15 ] , & ( & iobj_0 [ 0 ] ) [ 14 ] , &
( & iobj_1 [ 0 ] ) [ 7 ] ) ; } if ( rigidbodytree -> NumBodies >= 9.0 ) {
body = rigidbodytree -> Bodies [ 8 ] ; bid = body -> ParentIndex ; if ( bid >
0.0 ) { parent = rigidbodytree -> Bodies [ ( int32_T ) bid - 1 ] ; } else {
parent = & rigidbodytree -> Base ; } i = bname -> size [ 0 ] * bname -> size
[ 1 ] ; bname -> size [ 0 ] = 1 ; bname -> size [ 1 ] = parent ->
NameInternal -> size [ 1 ] ; e3n3mnwk4b ( bname , i ) ; loop_ub = parent ->
NameInternal -> size [ 1 ] ; for ( i = 0 ; i < loop_ub ; i ++ ) { bname ->
data [ i ] = parent -> NameInternal -> data [ i ] ; } jem4veejvh ( iobj_3 ,
body , bname , & ( & iobj_2 [ 0 ] ) [ 17 ] , & ( & iobj_0 [ 0 ] ) [ 16 ] , &
( & iobj_1 [ 0 ] ) [ 8 ] ) ; } fagcrr3avn ( & bname ) ; obj ->
RigidBodyTreeInternal = iobj_3 ; } static void ep2455cw5ms ( ecbwrsiemx * obj
) { alyuhn31fbfz * iobj_1 ; carpjyrt23 * obj_p ; ecfe5we0wc * iobj_4 ;
m0caqfktx0 * iobj_3 ; ogx4llc4arl * iobj_2 ; ot4uovzkt0w * iobj_0 ; int32_T i
; static const char_T tmp [ 22 ] = { 'B' , 'F' , 'G' , 'S' , 'G' , 'r' , 'a'
, 'd' , 'i' , 'e' , 'n' , 't' , 'P' , 'r' , 'o' , 'j' , 'e' , 'c' , 't' , 'i'
, 'o' , 'n' } ; static const mlxfszazeh tmp_p = { 0.0 , 0.0 } ; obj ->
isInitialized = 1 ; jgbwvk33lx ( & obj -> TreeInternal ) ; obj_p = & obj ->
IKInternal ; obj -> IKInternal . isInitialized = 0 ; iobj_0 = & obj ->
IKInternal . _pobj1 [ 0 ] ; iobj_1 = & obj -> IKInternal . _pobj2 [ 0 ] ;
iobj_2 = & obj -> IKInternal . _pobj3 [ 0 ] ; iobj_3 = & obj -> IKInternal .
_pobj4 ; iobj_4 = & obj -> IKInternal . _pobj5 ; omjnvaudda ( & obj ->
IKInternal , & obj -> TreeInternal , & ( & ( & iobj_0 [ 0 ] ) [ 0 ] ) [ 0 ] ,
& ( & ( & iobj_1 [ 0 ] ) [ 0 ] ) [ 0 ] , & ( & ( & iobj_2 [ 0 ] ) [ 0 ] ) [ 0
] , iobj_3 ) ; iobj_4 -> MaxNumIteration = 1500.0 ; iobj_4 -> MaxTime = 10.0
; iobj_4 -> GradientTolerance = 1.0E-7 ; iobj_4 -> SolutionTolerance = 1.0E-6
; iobj_4 -> ArmijoRuleBeta = 0.4 ; iobj_4 -> ArmijoRuleSigma = 1.0E-5 ;
iobj_4 -> ConstraintsOn = true ; iobj_4 -> RandomRestart = true ; iobj_4 ->
StepTolerance = 1.0E-14 ; for ( i = 0 ; i < 22 ; i ++ ) { iobj_4 -> Name [ i
] = tmp [ i ] ; } iobj_4 -> ConstraintMatrix -> size [ 0 ] = 0 ; iobj_4 ->
ConstraintMatrix -> size [ 1 ] = 0 ; iobj_4 -> ConstraintBound -> size [ 0 ]
= 0 ; iobj_4 -> TimeObj . StartTime = tmp_p ; iobj_4 -> TimeObjInternal .
StartTime = tmp_p ; iobj_4 -> matlabCodegenIsDeleted = false ; obj_p ->
Solver = iobj_4 ; iobj_4 = obj_p -> Solver ; iobj_4 -> MaxNumIteration = 30.0
; iobj_4 -> MaxTime = 10.0 ; iobj_4 -> GradientTolerance = 1.0E-7 ; iobj_4 ->
SolutionTolerance = 1.0E-6 ; iobj_4 -> ConstraintsOn = true ; iobj_4 ->
RandomRestart = false ; iobj_4 -> StepTolerance = 1.0E-14 ; obj_p ->
matlabCodegenIsDeleted = false ; } static void m4dtuohf4b ( ot4uovzkt0 *
pStruct ) { m2n1niwoq4 ( & pStruct -> Type , 2 ) ; } static void niy4nitoigo
( anhhcamq4s * * pEmxArray , int32_T numDimensions ) { anhhcamq4s * emxArray
; int32_T i ; * pEmxArray = ( anhhcamq4s * ) malloc ( sizeof ( anhhcamq4s ) )
; emxArray = * pEmxArray ; emxArray -> data = ( om2wdj4oun * ) NULL ;
emxArray -> numDimensions = numDimensions ; emxArray -> size = ( int32_T * )
malloc ( sizeof ( int32_T ) * ( uint32_T ) numDimensions ) ; emxArray ->
allocatedSize = 0 ; emxArray -> canFreeData = true ; for ( i = 0 ; i <
numDimensions ; i ++ ) { emxArray -> size [ i ] = 0 ; } } static void
iwumdx5q3w ( ogx4llc4ar * pStruct ) { niy4nitoigo ( & pStruct ->
CollisionGeometries , 2 ) ; } static void nwwh5wd3rb ( alyuhn31fb * pStruct )
{ m2n1niwoq4 ( & pStruct -> NameInternal , 2 ) ; m4dtuohf4b ( & pStruct ->
JointInternal ) ; iwumdx5q3w ( & pStruct -> CollisionsInternal ) ; } static
void ezyujupqoplefp ( alyuhn31fb pMatrix [ 18 ] ) { int32_T i ; for ( i = 0 ;
i < 18 ; i ++ ) { nwwh5wd3rb ( & pMatrix [ i ] ) ; } } static void
iz5okq0a3xik4sr ( mjv2tzlyyi * pStruct ) { nwwh5wd3rb ( & pStruct -> Base ) ;
ezyujupqoplefp ( pStruct -> _pobj0 ) ; } static void ecqu5ikgmie ( evzhsot2uw
* pStruct ) { iz5okq0a3xik4sr ( & pStruct -> TreeInternal ) ; } static void
aupymb1nfg ( anhhcamq4s * emxArray , int32_T oldNumel ) { int32_T i ; int32_T
newNumel ; void * newData ; if ( oldNumel < 0 ) { oldNumel = 0 ; } newNumel =
1 ; for ( i = 0 ; i < emxArray -> numDimensions ; i ++ ) { newNumel *=
emxArray -> size [ i ] ; } if ( newNumel > emxArray -> allocatedSize ) { i =
emxArray -> allocatedSize ; if ( i < 16 ) { i = 16 ; } while ( i < newNumel )
{ if ( i > 1073741823 ) { i = MAX_int32_T ; } else { i <<= 1 ; } } newData =
calloc ( ( uint32_T ) i , sizeof ( om2wdj4oun ) ) ; if ( emxArray -> data !=
NULL ) { memcpy ( newData , emxArray -> data , sizeof ( om2wdj4oun ) * (
uint32_T ) oldNumel ) ; if ( emxArray -> canFreeData ) { free ( emxArray ->
data ) ; } } emxArray -> data = ( om2wdj4oun * ) newData ; emxArray ->
allocatedSize = i ; emxArray -> canFreeData = true ; } } static void
oamrpiq5obm ( anhhcamq4s * * pEmxArray ) { if ( * pEmxArray != ( anhhcamq4s *
) NULL ) { if ( ( ( * pEmxArray ) -> data != ( om2wdj4oun * ) NULL ) && ( *
pEmxArray ) -> canFreeData ) { free ( ( * pEmxArray ) -> data ) ; } free ( (
* pEmxArray ) -> size ) ; free ( * pEmxArray ) ; * pEmxArray = ( anhhcamq4s *
) NULL ; } } static ogx4llc4ar * p1kacjwiri ( ogx4llc4ar * obj ) { void *
defaultCollisionObj_GeometryInternal ; anhhcamq4s * e ; ogx4llc4ar * b_obj ;
real_T c ; int32_T b_i ; int32_T d ; obj -> Size = 0.0 ; b_obj = obj ; obj ->
MaxElements = 0.0 ; niy4nitoigo ( & e , 2 ) ; b_i = e -> size [ 0 ] * e ->
size [ 1 ] ; e -> size [ 1 ] = ( int32_T ) obj -> MaxElements ; aupymb1nfg (
e , b_i ) ; b_i = obj -> CollisionGeometries -> size [ 0 ] * obj ->
CollisionGeometries -> size [ 1 ] ; obj -> CollisionGeometries -> size [ 0 ]
= 1 ; obj -> CollisionGeometries -> size [ 1 ] = e -> size [ 1 ] ;
oamrpiq5obm ( & e ) ; aupymb1nfg ( obj -> CollisionGeometries , b_i ) ;
defaultCollisionObj_GeometryInternal = NULL ; c = obj -> MaxElements ; d = (
int32_T ) c - 1 ; for ( b_i = 0 ; b_i <= d ; b_i ++ ) { obj ->
CollisionGeometries -> data [ b_i ] . CollisionPrimitive =
defaultCollisionObj_GeometryInternal ; } obj -> matlabCodegenIsDeleted =
false ; return b_obj ; } static alyuhn31fb * nnpwrpxrzk ( alyuhn31fb * obj )
{ alyuhn31fb * b_obj ; e1n154mvbh * switch_expression ; int32_T b_kstr ;
int32_T loop_ub ; char_T b_p [ 9 ] ; char_T b [ 8 ] ; boolean_T b_bool ;
static const char_T tmp [ 10 ] = { 'd' , 'u' , 'm' , 'm' , 'y' , 'b' , 'o' ,
'd' , 'y' , '5' } ; static const char_T tmp_p [ 5 ] = { 'f' , 'i' , 'x' , 'e'
, 'd' } ; static const char_T tmp_e [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l' ,
'u' , 't' , 'e' } ; static const char_T tmp_i [ 9 ] = { 'p' , 'r' , 'i' , 's'
, 'm' , 'a' , 't' , 'i' , 'c' } ; int32_T exitg1 ; b_obj = obj ; b_kstr = obj
-> NameInternal -> size [ 0 ] * obj -> NameInternal -> size [ 1 ] ; obj ->
NameInternal -> size [ 0 ] = 1 ; obj -> NameInternal -> size [ 1 ] = 10 ;
e3n3mnwk4b ( obj -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 10
; b_kstr ++ ) { obj -> NameInternal -> data [ b_kstr ] = tmp [ b_kstr ] ; }
b_kstr = obj -> JointInternal . Type -> size [ 0 ] * obj -> JointInternal .
Type -> size [ 1 ] ; obj -> JointInternal . Type -> size [ 0 ] = 1 ; obj ->
JointInternal . Type -> size [ 1 ] = 5 ; e3n3mnwk4b ( obj -> JointInternal .
Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { obj ->
JointInternal . Type -> data [ b_kstr ] = tmp_p [ b_kstr ] ; } m2n1niwoq4 ( &
switch_expression , 2 ) ; b_kstr = switch_expression -> size [ 0 ] *
switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = obj -> JointInternal . Type -> size [ 1 ] ;
e3n3mnwk4b ( switch_expression , b_kstr ) ; loop_ub = obj -> JointInternal .
Type -> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) {
switch_expression -> data [ b_kstr ] = obj -> JointInternal . Type -> data [
b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { b [ b_kstr ] =
tmp_e [ b_kstr ] ; } b_bool = false ; if ( switch_expression -> size [ 1 ] !=
8 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if (
switch_expression -> data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1 ;
} else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0 ; } else { for ( b_kstr = 0 ;
b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_i [ b_kstr ] ; } if (
switch_expression -> size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1
; } else { b_kstr = - 1 ; } } fagcrr3avn ( & switch_expression ) ; switch (
b_kstr ) { case 0 : obj -> JointInternal . PositionNumber = 1.0 ; obj ->
JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2
] = 1.0 ; break ; case 1 : obj -> JointInternal . PositionNumber = 1.0 ; obj
-> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2
] = 1.0 ; break ; default : obj -> JointInternal . PositionNumber = 0.0 ; obj
-> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2
] = 0.0 ; break ; } obj -> ParentIndex = - 1.0 ; p1kacjwiri ( & obj ->
CollisionsInternal ) ; obj -> matlabCodegenIsDeleted = false ; return b_obj ;
} static alyuhn31fb * nnpwrpxrzkq ( alyuhn31fb * obj ) { alyuhn31fb * b_obj ;
e1n154mvbh * switch_expression ; int32_T b_kstr ; int32_T loop_ub ; char_T
b_p [ 9 ] ; char_T b [ 8 ] ; boolean_T b_bool ; static const char_T tmp [ 10
] = { 'd' , 'u' , 'm' , 'm' , 'y' , 'b' , 'o' , 'd' , 'y' , '6' } ; static
const char_T tmp_p [ 5 ] = { 'f' , 'i' , 'x' , 'e' , 'd' } ; static const
char_T tmp_e [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ;
static const char_T tmp_i [ 9 ] = { 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' ,
'i' , 'c' } ; int32_T exitg1 ; b_obj = obj ; b_kstr = obj -> NameInternal ->
size [ 0 ] * obj -> NameInternal -> size [ 1 ] ; obj -> NameInternal -> size
[ 0 ] = 1 ; obj -> NameInternal -> size [ 1 ] = 10 ; e3n3mnwk4b ( obj ->
NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 10 ; b_kstr ++ ) { obj
-> NameInternal -> data [ b_kstr ] = tmp [ b_kstr ] ; } b_kstr = obj ->
JointInternal . Type -> size [ 0 ] * obj -> JointInternal . Type -> size [ 1
] ; obj -> JointInternal . Type -> size [ 0 ] = 1 ; obj -> JointInternal .
Type -> size [ 1 ] = 5 ; e3n3mnwk4b ( obj -> JointInternal . Type , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { obj -> JointInternal . Type
-> data [ b_kstr ] = tmp_p [ b_kstr ] ; } m2n1niwoq4 ( & switch_expression ,
2 ) ; b_kstr = switch_expression -> size [ 0 ] * switch_expression -> size [
1 ] ; switch_expression -> size [ 0 ] = 1 ; switch_expression -> size [ 1 ] =
obj -> JointInternal . Type -> size [ 1 ] ; e3n3mnwk4b ( switch_expression ,
b_kstr ) ; loop_ub = obj -> JointInternal . Type -> size [ 1 ] ; for ( b_kstr
= 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [ b_kstr ] =
obj -> JointInternal . Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr
< 8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_e [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_i [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : obj -> JointInternal .
PositionNumber = 1.0 ; obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ;
obj -> JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; case 1 : obj -> JointInternal .
PositionNumber = 1.0 ; obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ;
obj -> JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; default : obj -> JointInternal .
PositionNumber = 0.0 ; obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ;
obj -> JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 0.0 ; break ; } obj -> ParentIndex = - 1.0 ;
p1kacjwiri ( & obj -> CollisionsInternal ) ; obj -> matlabCodegenIsDeleted =
false ; return b_obj ; } static alyuhn31fb * nnpwrpxrzkqi ( alyuhn31fb * obj
) { alyuhn31fb * b_obj ; e1n154mvbh * switch_expression ; int32_T b_kstr ;
int32_T loop_ub ; char_T b_p [ 9 ] ; char_T b [ 8 ] ; boolean_T b_bool ;
static const char_T tmp [ 10 ] = { 'd' , 'u' , 'm' , 'm' , 'y' , 'b' , 'o' ,
'd' , 'y' , '7' } ; static const char_T tmp_p [ 5 ] = { 'f' , 'i' , 'x' , 'e'
, 'd' } ; static const char_T tmp_e [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l' ,
'u' , 't' , 'e' } ; static const char_T tmp_i [ 9 ] = { 'p' , 'r' , 'i' , 's'
, 'm' , 'a' , 't' , 'i' , 'c' } ; int32_T exitg1 ; b_obj = obj ; b_kstr = obj
-> NameInternal -> size [ 0 ] * obj -> NameInternal -> size [ 1 ] ; obj ->
NameInternal -> size [ 0 ] = 1 ; obj -> NameInternal -> size [ 1 ] = 10 ;
e3n3mnwk4b ( obj -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 10
; b_kstr ++ ) { obj -> NameInternal -> data [ b_kstr ] = tmp [ b_kstr ] ; }
b_kstr = obj -> JointInternal . Type -> size [ 0 ] * obj -> JointInternal .
Type -> size [ 1 ] ; obj -> JointInternal . Type -> size [ 0 ] = 1 ; obj ->
JointInternal . Type -> size [ 1 ] = 5 ; e3n3mnwk4b ( obj -> JointInternal .
Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { obj ->
JointInternal . Type -> data [ b_kstr ] = tmp_p [ b_kstr ] ; } m2n1niwoq4 ( &
switch_expression , 2 ) ; b_kstr = switch_expression -> size [ 0 ] *
switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = obj -> JointInternal . Type -> size [ 1 ] ;
e3n3mnwk4b ( switch_expression , b_kstr ) ; loop_ub = obj -> JointInternal .
Type -> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) {
switch_expression -> data [ b_kstr ] = obj -> JointInternal . Type -> data [
b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { b [ b_kstr ] =
tmp_e [ b_kstr ] ; } b_bool = false ; if ( switch_expression -> size [ 1 ] !=
8 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if (
switch_expression -> data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1 ;
} else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0 ; } else { for ( b_kstr = 0 ;
b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_i [ b_kstr ] ; } if (
switch_expression -> size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1
; } else { b_kstr = - 1 ; } } fagcrr3avn ( & switch_expression ) ; switch (
b_kstr ) { case 0 : obj -> JointInternal . PositionNumber = 1.0 ; obj ->
JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2
] = 1.0 ; break ; case 1 : obj -> JointInternal . PositionNumber = 1.0 ; obj
-> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2
] = 1.0 ; break ; default : obj -> JointInternal . PositionNumber = 0.0 ; obj
-> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2
] = 0.0 ; break ; } obj -> ParentIndex = - 1.0 ; p1kacjwiri ( & obj ->
CollisionsInternal ) ; obj -> matlabCodegenIsDeleted = false ; return b_obj ;
} static alyuhn31fb * nnpwrpxrzkqix ( alyuhn31fb * obj ) { alyuhn31fb * b_obj
; e1n154mvbh * switch_expression ; int32_T b_kstr ; int32_T loop_ub ; char_T
b_p [ 9 ] ; char_T b [ 8 ] ; boolean_T b_bool ; static const char_T tmp [ 10
] = { 'd' , 'u' , 'm' , 'm' , 'y' , 'b' , 'o' , 'd' , 'y' , '8' } ; static
const char_T tmp_p [ 5 ] = { 'f' , 'i' , 'x' , 'e' , 'd' } ; static const
char_T tmp_e [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ;
static const char_T tmp_i [ 9 ] = { 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' ,
'i' , 'c' } ; int32_T exitg1 ; b_obj = obj ; b_kstr = obj -> NameInternal ->
size [ 0 ] * obj -> NameInternal -> size [ 1 ] ; obj -> NameInternal -> size
[ 0 ] = 1 ; obj -> NameInternal -> size [ 1 ] = 10 ; e3n3mnwk4b ( obj ->
NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 10 ; b_kstr ++ ) { obj
-> NameInternal -> data [ b_kstr ] = tmp [ b_kstr ] ; } b_kstr = obj ->
JointInternal . Type -> size [ 0 ] * obj -> JointInternal . Type -> size [ 1
] ; obj -> JointInternal . Type -> size [ 0 ] = 1 ; obj -> JointInternal .
Type -> size [ 1 ] = 5 ; e3n3mnwk4b ( obj -> JointInternal . Type , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { obj -> JointInternal . Type
-> data [ b_kstr ] = tmp_p [ b_kstr ] ; } m2n1niwoq4 ( & switch_expression ,
2 ) ; b_kstr = switch_expression -> size [ 0 ] * switch_expression -> size [
1 ] ; switch_expression -> size [ 0 ] = 1 ; switch_expression -> size [ 1 ] =
obj -> JointInternal . Type -> size [ 1 ] ; e3n3mnwk4b ( switch_expression ,
b_kstr ) ; loop_ub = obj -> JointInternal . Type -> size [ 1 ] ; for ( b_kstr
= 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [ b_kstr ] =
obj -> JointInternal . Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr
< 8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_e [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_i [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : obj -> JointInternal .
PositionNumber = 1.0 ; obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ;
obj -> JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; case 1 : obj -> JointInternal .
PositionNumber = 1.0 ; obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ;
obj -> JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; default : obj -> JointInternal .
PositionNumber = 0.0 ; obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ;
obj -> JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 0.0 ; break ; } obj -> ParentIndex = - 1.0 ;
p1kacjwiri ( & obj -> CollisionsInternal ) ; obj -> matlabCodegenIsDeleted =
false ; return b_obj ; } static alyuhn31fb * nnpwrpxrzkqixz ( alyuhn31fb *
obj ) { alyuhn31fb * b_obj ; e1n154mvbh * switch_expression ; int32_T b_kstr
; int32_T loop_ub ; char_T b_p [ 9 ] ; char_T b [ 8 ] ; boolean_T b_bool ;
static const char_T tmp [ 10 ] = { 'd' , 'u' , 'm' , 'm' , 'y' , 'b' , 'o' ,
'd' , 'y' , '9' } ; static const char_T tmp_p [ 5 ] = { 'f' , 'i' , 'x' , 'e'
, 'd' } ; static const char_T tmp_e [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l' ,
'u' , 't' , 'e' } ; static const char_T tmp_i [ 9 ] = { 'p' , 'r' , 'i' , 's'
, 'm' , 'a' , 't' , 'i' , 'c' } ; int32_T exitg1 ; b_obj = obj ; b_kstr = obj
-> NameInternal -> size [ 0 ] * obj -> NameInternal -> size [ 1 ] ; obj ->
NameInternal -> size [ 0 ] = 1 ; obj -> NameInternal -> size [ 1 ] = 10 ;
e3n3mnwk4b ( obj -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 10
; b_kstr ++ ) { obj -> NameInternal -> data [ b_kstr ] = tmp [ b_kstr ] ; }
b_kstr = obj -> JointInternal . Type -> size [ 0 ] * obj -> JointInternal .
Type -> size [ 1 ] ; obj -> JointInternal . Type -> size [ 0 ] = 1 ; obj ->
JointInternal . Type -> size [ 1 ] = 5 ; e3n3mnwk4b ( obj -> JointInternal .
Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { obj ->
JointInternal . Type -> data [ b_kstr ] = tmp_p [ b_kstr ] ; } m2n1niwoq4 ( &
switch_expression , 2 ) ; b_kstr = switch_expression -> size [ 0 ] *
switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = obj -> JointInternal . Type -> size [ 1 ] ;
e3n3mnwk4b ( switch_expression , b_kstr ) ; loop_ub = obj -> JointInternal .
Type -> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) {
switch_expression -> data [ b_kstr ] = obj -> JointInternal . Type -> data [
b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { b [ b_kstr ] =
tmp_e [ b_kstr ] ; } b_bool = false ; if ( switch_expression -> size [ 1 ] !=
8 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if (
switch_expression -> data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1 ;
} else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0 ; } else { for ( b_kstr = 0 ;
b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_i [ b_kstr ] ; } if (
switch_expression -> size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1
; } else { b_kstr = - 1 ; } } fagcrr3avn ( & switch_expression ) ; switch (
b_kstr ) { case 0 : obj -> JointInternal . PositionNumber = 1.0 ; obj ->
JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2
] = 1.0 ; break ; case 1 : obj -> JointInternal . PositionNumber = 1.0 ; obj
-> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2
] = 1.0 ; break ; default : obj -> JointInternal . PositionNumber = 0.0 ; obj
-> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2
] = 0.0 ; break ; } obj -> ParentIndex = - 1.0 ; p1kacjwiri ( & obj ->
CollisionsInternal ) ; obj -> matlabCodegenIsDeleted = false ; return b_obj ;
} static void kff4ccpwjv ( mjv2tzlyyi * obj , alyuhn31fb * iobj_0 ) {
e1n154mvbh * switch_expression ; int32_T b_kstr ; int32_T loop_ub ; char_T
b_p [ 9 ] ; char_T b [ 8 ] ; boolean_T b_bool ; static const char_T tmp [ 10
] = { 'd' , 'u' , 'm' , 'm' , 'y' , 'b' , 'o' , 'd' , 'y' , '1' } ; static
const char_T tmp_p [ 5 ] = { 'f' , 'i' , 'x' , 'e' , 'd' } ; static const
char_T tmp_e [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ;
static const char_T tmp_i [ 9 ] = { 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' ,
'i' , 'c' } ; static const char_T tmp_m [ 10 ] = { 'd' , 'u' , 'm' , 'm' ,
'y' , 'b' , 'o' , 'd' , 'y' , '2' } ; static const char_T tmp_g [ 10 ] = {
'd' , 'u' , 'm' , 'm' , 'y' , 'b' , 'o' , 'd' , 'y' , '3' } ; static const
char_T tmp_j [ 10 ] = { 'd' , 'u' , 'm' , 'm' , 'y' , 'b' , 'o' , 'd' , 'y' ,
'4' } ; int32_T exitg1 ; b_kstr = iobj_0 [ 0 ] . NameInternal -> size [ 0 ] *
iobj_0 [ 0 ] . NameInternal -> size [ 1 ] ; iobj_0 [ 0 ] . NameInternal ->
size [ 0 ] = 1 ; iobj_0 [ 0 ] . NameInternal -> size [ 1 ] = 10 ; e3n3mnwk4b
( iobj_0 [ 0 ] . NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 10 ;
b_kstr ++ ) { iobj_0 [ 0 ] . NameInternal -> data [ b_kstr ] = tmp [ b_kstr ]
; } b_kstr = iobj_0 [ 0 ] . JointInternal . Type -> size [ 0 ] * iobj_0 [ 0 ]
. JointInternal . Type -> size [ 1 ] ; iobj_0 [ 0 ] . JointInternal . Type ->
size [ 0 ] = 1 ; iobj_0 [ 0 ] . JointInternal . Type -> size [ 1 ] = 5 ;
e3n3mnwk4b ( iobj_0 [ 0 ] . JointInternal . Type , b_kstr ) ; for ( b_kstr =
0 ; b_kstr < 5 ; b_kstr ++ ) { iobj_0 [ 0 ] . JointInternal . Type -> data [
b_kstr ] = tmp_p [ b_kstr ] ; } m2n1niwoq4 ( & switch_expression , 2 ) ;
b_kstr = switch_expression -> size [ 0 ] * switch_expression -> size [ 1 ] ;
switch_expression -> size [ 0 ] = 1 ; switch_expression -> size [ 1 ] =
iobj_0 [ 0 ] . JointInternal . Type -> size [ 1 ] ; e3n3mnwk4b (
switch_expression , b_kstr ) ; loop_ub = iobj_0 [ 0 ] . JointInternal . Type
-> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) {
switch_expression -> data [ b_kstr ] = iobj_0 [ 0 ] . JointInternal . Type ->
data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { b [ b_kstr
] = tmp_e [ b_kstr ] ; } b_bool = false ; if ( switch_expression -> size [ 1
] != 8 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if
( switch_expression -> data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1
; } else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0 ; } else { for ( b_kstr = 0 ;
b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_i [ b_kstr ] ; } if (
switch_expression -> size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1
; } else { b_kstr = - 1 ; } } switch ( b_kstr ) { case 0 : iobj_0 [ 0 ] .
JointInternal . PositionNumber = 1.0 ; iobj_0 [ 0 ] . JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; iobj_0 [ 0 ] . JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; iobj_0 [ 0 ] . JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; case 1 : iobj_0 [ 0 ] . JointInternal
. PositionNumber = 1.0 ; iobj_0 [ 0 ] . JointInternal . JointAxisInternal [ 0
] = 0.0 ; iobj_0 [ 0 ] . JointInternal . JointAxisInternal [ 1 ] = 0.0 ;
iobj_0 [ 0 ] . JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ;
default : iobj_0 [ 0 ] . JointInternal . PositionNumber = 0.0 ; iobj_0 [ 0 ]
. JointInternal . JointAxisInternal [ 0 ] = 0.0 ; iobj_0 [ 0 ] .
JointInternal . JointAxisInternal [ 1 ] = 0.0 ; iobj_0 [ 0 ] . JointInternal
. JointAxisInternal [ 2 ] = 0.0 ; break ; } iobj_0 [ 0 ] . ParentIndex = -
1.0 ; p1kacjwiri ( & iobj_0 [ 0 ] . CollisionsInternal ) ; iobj_0 [ 0 ] .
matlabCodegenIsDeleted = false ; obj -> Bodies [ 0 ] = & iobj_0 [ 0 ] ;
b_kstr = iobj_0 [ 1 ] . NameInternal -> size [ 0 ] * iobj_0 [ 1 ] .
NameInternal -> size [ 1 ] ; iobj_0 [ 1 ] . NameInternal -> size [ 0 ] = 1 ;
iobj_0 [ 1 ] . NameInternal -> size [ 1 ] = 10 ; e3n3mnwk4b ( iobj_0 [ 1 ] .
NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 10 ; b_kstr ++ ) {
iobj_0 [ 1 ] . NameInternal -> data [ b_kstr ] = tmp_m [ b_kstr ] ; } b_kstr
= iobj_0 [ 1 ] . JointInternal . Type -> size [ 0 ] * iobj_0 [ 1 ] .
JointInternal . Type -> size [ 1 ] ; iobj_0 [ 1 ] . JointInternal . Type ->
size [ 0 ] = 1 ; iobj_0 [ 1 ] . JointInternal . Type -> size [ 1 ] = 5 ;
e3n3mnwk4b ( iobj_0 [ 1 ] . JointInternal . Type , b_kstr ) ; for ( b_kstr =
0 ; b_kstr < 5 ; b_kstr ++ ) { iobj_0 [ 1 ] . JointInternal . Type -> data [
b_kstr ] = tmp_p [ b_kstr ] ; } b_kstr = switch_expression -> size [ 0 ] *
switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = iobj_0 [ 1 ] . JointInternal . Type -> size
[ 1 ] ; e3n3mnwk4b ( switch_expression , b_kstr ) ; loop_ub = iobj_0 [ 1 ] .
JointInternal . Type -> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ;
b_kstr ++ ) { switch_expression -> data [ b_kstr ] = iobj_0 [ 1 ] .
JointInternal . Type -> data [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_i [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } switch ( b_kstr ) {
case 0 : iobj_0 [ 1 ] . JointInternal . PositionNumber = 1.0 ; iobj_0 [ 1 ] .
JointInternal . JointAxisInternal [ 0 ] = 0.0 ; iobj_0 [ 1 ] . JointInternal
. JointAxisInternal [ 1 ] = 0.0 ; iobj_0 [ 1 ] . JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; case 1 : iobj_0 [ 1 ] . JointInternal
. PositionNumber = 1.0 ; iobj_0 [ 1 ] . JointInternal . JointAxisInternal [ 0
] = 0.0 ; iobj_0 [ 1 ] . JointInternal . JointAxisInternal [ 1 ] = 0.0 ;
iobj_0 [ 1 ] . JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ;
default : iobj_0 [ 1 ] . JointInternal . PositionNumber = 0.0 ; iobj_0 [ 1 ]
. JointInternal . JointAxisInternal [ 0 ] = 0.0 ; iobj_0 [ 1 ] .
JointInternal . JointAxisInternal [ 1 ] = 0.0 ; iobj_0 [ 1 ] . JointInternal
. JointAxisInternal [ 2 ] = 0.0 ; break ; } iobj_0 [ 1 ] . ParentIndex = -
1.0 ; p1kacjwiri ( & iobj_0 [ 1 ] . CollisionsInternal ) ; iobj_0 [ 1 ] .
matlabCodegenIsDeleted = false ; obj -> Bodies [ 1 ] = & iobj_0 [ 1 ] ;
b_kstr = iobj_0 [ 2 ] . NameInternal -> size [ 0 ] * iobj_0 [ 2 ] .
NameInternal -> size [ 1 ] ; iobj_0 [ 2 ] . NameInternal -> size [ 0 ] = 1 ;
iobj_0 [ 2 ] . NameInternal -> size [ 1 ] = 10 ; e3n3mnwk4b ( iobj_0 [ 2 ] .
NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 10 ; b_kstr ++ ) {
iobj_0 [ 2 ] . NameInternal -> data [ b_kstr ] = tmp_g [ b_kstr ] ; } b_kstr
= iobj_0 [ 2 ] . JointInternal . Type -> size [ 0 ] * iobj_0 [ 2 ] .
JointInternal . Type -> size [ 1 ] ; iobj_0 [ 2 ] . JointInternal . Type ->
size [ 0 ] = 1 ; iobj_0 [ 2 ] . JointInternal . Type -> size [ 1 ] = 5 ;
e3n3mnwk4b ( iobj_0 [ 2 ] . JointInternal . Type , b_kstr ) ; for ( b_kstr =
0 ; b_kstr < 5 ; b_kstr ++ ) { iobj_0 [ 2 ] . JointInternal . Type -> data [
b_kstr ] = tmp_p [ b_kstr ] ; } b_kstr = switch_expression -> size [ 0 ] *
switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = iobj_0 [ 2 ] . JointInternal . Type -> size
[ 1 ] ; e3n3mnwk4b ( switch_expression , b_kstr ) ; loop_ub = iobj_0 [ 2 ] .
JointInternal . Type -> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ;
b_kstr ++ ) { switch_expression -> data [ b_kstr ] = iobj_0 [ 2 ] .
JointInternal . Type -> data [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_i [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } switch ( b_kstr ) {
case 0 : iobj_0 [ 2 ] . JointInternal . PositionNumber = 1.0 ; iobj_0 [ 2 ] .
JointInternal . JointAxisInternal [ 0 ] = 0.0 ; iobj_0 [ 2 ] . JointInternal
. JointAxisInternal [ 1 ] = 0.0 ; iobj_0 [ 2 ] . JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; case 1 : iobj_0 [ 2 ] . JointInternal
. PositionNumber = 1.0 ; iobj_0 [ 2 ] . JointInternal . JointAxisInternal [ 0
] = 0.0 ; iobj_0 [ 2 ] . JointInternal . JointAxisInternal [ 1 ] = 0.0 ;
iobj_0 [ 2 ] . JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ;
default : iobj_0 [ 2 ] . JointInternal . PositionNumber = 0.0 ; iobj_0 [ 2 ]
. JointInternal . JointAxisInternal [ 0 ] = 0.0 ; iobj_0 [ 2 ] .
JointInternal . JointAxisInternal [ 1 ] = 0.0 ; iobj_0 [ 2 ] . JointInternal
. JointAxisInternal [ 2 ] = 0.0 ; break ; } iobj_0 [ 2 ] . ParentIndex = -
1.0 ; p1kacjwiri ( & iobj_0 [ 2 ] . CollisionsInternal ) ; iobj_0 [ 2 ] .
matlabCodegenIsDeleted = false ; obj -> Bodies [ 2 ] = & iobj_0 [ 2 ] ;
b_kstr = iobj_0 [ 3 ] . NameInternal -> size [ 0 ] * iobj_0 [ 3 ] .
NameInternal -> size [ 1 ] ; iobj_0 [ 3 ] . NameInternal -> size [ 0 ] = 1 ;
iobj_0 [ 3 ] . NameInternal -> size [ 1 ] = 10 ; e3n3mnwk4b ( iobj_0 [ 3 ] .
NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 10 ; b_kstr ++ ) {
iobj_0 [ 3 ] . NameInternal -> data [ b_kstr ] = tmp_j [ b_kstr ] ; } b_kstr
= iobj_0 [ 3 ] . JointInternal . Type -> size [ 0 ] * iobj_0 [ 3 ] .
JointInternal . Type -> size [ 1 ] ; iobj_0 [ 3 ] . JointInternal . Type ->
size [ 0 ] = 1 ; iobj_0 [ 3 ] . JointInternal . Type -> size [ 1 ] = 5 ;
e3n3mnwk4b ( iobj_0 [ 3 ] . JointInternal . Type , b_kstr ) ; for ( b_kstr =
0 ; b_kstr < 5 ; b_kstr ++ ) { iobj_0 [ 3 ] . JointInternal . Type -> data [
b_kstr ] = tmp_p [ b_kstr ] ; } b_kstr = switch_expression -> size [ 0 ] *
switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = iobj_0 [ 3 ] . JointInternal . Type -> size
[ 1 ] ; e3n3mnwk4b ( switch_expression , b_kstr ) ; loop_ub = iobj_0 [ 3 ] .
JointInternal . Type -> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ;
b_kstr ++ ) { switch_expression -> data [ b_kstr ] = iobj_0 [ 3 ] .
JointInternal . Type -> data [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_i [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : iobj_0 [ 3 ] .
JointInternal . PositionNumber = 1.0 ; iobj_0 [ 3 ] . JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; iobj_0 [ 3 ] . JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; iobj_0 [ 3 ] . JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; case 1 : iobj_0 [ 3 ] . JointInternal
. PositionNumber = 1.0 ; iobj_0 [ 3 ] . JointInternal . JointAxisInternal [ 0
] = 0.0 ; iobj_0 [ 3 ] . JointInternal . JointAxisInternal [ 1 ] = 0.0 ;
iobj_0 [ 3 ] . JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ;
default : iobj_0 [ 3 ] . JointInternal . PositionNumber = 0.0 ; iobj_0 [ 3 ]
. JointInternal . JointAxisInternal [ 0 ] = 0.0 ; iobj_0 [ 3 ] .
JointInternal . JointAxisInternal [ 1 ] = 0.0 ; iobj_0 [ 3 ] . JointInternal
. JointAxisInternal [ 2 ] = 0.0 ; break ; } iobj_0 [ 3 ] . ParentIndex = -
1.0 ; p1kacjwiri ( & iobj_0 [ 3 ] . CollisionsInternal ) ; iobj_0 [ 3 ] .
matlabCodegenIsDeleted = false ; obj -> Bodies [ 3 ] = & iobj_0 [ 3 ] ; obj
-> Bodies [ 4 ] = nnpwrpxrzk ( & iobj_0 [ 4 ] ) ; obj -> Bodies [ 5 ] =
nnpwrpxrzkq ( & iobj_0 [ 5 ] ) ; obj -> Bodies [ 6 ] = nnpwrpxrzkqi ( &
iobj_0 [ 6 ] ) ; obj -> Bodies [ 7 ] = nnpwrpxrzkqix ( & iobj_0 [ 7 ] ) ; obj
-> Bodies [ 8 ] = nnpwrpxrzkqixz ( & iobj_0 [ 8 ] ) ; } static alyuhn31fb *
nnpwrpxrzkqixzq ( alyuhn31fb * obj ) { alyuhn31fb * b_obj ; e1n154mvbh *
switch_expression ; int32_T b_kstr ; int32_T loop_ub ; char_T b_p [ 9 ] ;
char_T b [ 8 ] ; boolean_T b_bool ; static const char_T tmp [ 5 ] = { 'l' ,
'i' , 'n' , 'k' , '4' } ; static const char_T tmp_p [ 8 ] = { 'r' , 'e' , 'v'
, 'o' , 'l' , 'u' , 't' , 'e' } ; static const char_T tmp_e [ 9 ] = { 'p' ,
'r' , 'i' , 's' , 'm' , 'a' , 't' , 'i' , 'c' } ; static const real_T tmp_i [
16 ] = { 1.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 ,
0.0 , 0.024 , 0.0 , 0.128 , 1.0 } ; static const real_T tmp_m [ 16 ] = { 1.0
, 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0
, 0.0 , 1.0 } ; int32_T exitg1 ; b_obj = obj ; b_kstr = obj -> NameInternal
-> size [ 0 ] * obj -> NameInternal -> size [ 1 ] ; obj -> NameInternal ->
size [ 0 ] = 1 ; obj -> NameInternal -> size [ 1 ] = 5 ; e3n3mnwk4b ( obj ->
NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { obj
-> NameInternal -> data [ b_kstr ] = tmp [ b_kstr ] ; } obj -> ParentIndex =
3.0 ; b_kstr = obj -> JointInternal . Type -> size [ 0 ] * obj ->
JointInternal . Type -> size [ 1 ] ; obj -> JointInternal . Type -> size [ 0
] = 1 ; obj -> JointInternal . Type -> size [ 1 ] = 8 ; e3n3mnwk4b ( obj ->
JointInternal . Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ )
{ obj -> JointInternal . Type -> data [ b_kstr ] = tmp_p [ b_kstr ] ; }
m2n1niwoq4 ( & switch_expression , 2 ) ; b_kstr = switch_expression -> size [
0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = obj -> JointInternal . Type -> size [ 1 ] ;
e3n3mnwk4b ( switch_expression , b_kstr ) ; loop_ub = obj -> JointInternal .
Type -> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) {
switch_expression -> data [ b_kstr ] = obj -> JointInternal . Type -> data [
b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { b [ b_kstr ] =
tmp_p [ b_kstr ] ; } b_bool = false ; if ( switch_expression -> size [ 1 ] !=
8 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if (
switch_expression -> data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1 ;
} else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0 ; } else { for ( b_kstr = 0 ;
b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_e [ b_kstr ] ; } if (
switch_expression -> size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1
; } else { b_kstr = - 1 ; } } fagcrr3avn ( & switch_expression ) ; switch (
b_kstr ) { case 0 : obj -> JointInternal . PositionNumber = 1.0 ; obj ->
JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2
] = 1.0 ; break ; case 1 : obj -> JointInternal . PositionNumber = 1.0 ; obj
-> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2
] = 1.0 ; break ; default : obj -> JointInternal . PositionNumber = 0.0 ; obj
-> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2
] = 0.0 ; break ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj ->
JointInternal . JointToParentTransform [ b_kstr ] = tmp_i [ b_kstr ] ; } for
( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj -> JointInternal .
ChildToJointTransform [ b_kstr ] = tmp_m [ b_kstr ] ; } obj -> JointInternal
. JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [
1 ] = 1.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 0.0 ; p1kacjwiri
( & obj -> CollisionsInternal ) ; obj -> matlabCodegenIsDeleted = false ;
return b_obj ; } static alyuhn31fb * b4tj0yrbq0 ( alyuhn31fb * obj ) {
alyuhn31fb * b_obj ; e1n154mvbh * switch_expression ; int32_T b_kstr ;
int32_T loop_ub ; char_T b_p [ 9 ] ; char_T b [ 8 ] ; boolean_T b_bool ;
static const char_T tmp [ 5 ] = { 'l' , 'i' , 'n' , 'k' , '5' } ; static
const char_T tmp_p [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' }
; static const char_T tmp_e [ 9 ] = { 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't'
, 'i' , 'c' } ; static const real_T tmp_i [ 16 ] = { 1.0 , 0.0 , - 0.0 , 0.0
, 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.124 , 0.0 , 0.0 , 1.0 } ;
static const real_T tmp_m [ 16 ] = { 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0
, 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 } ; int32_T exitg1 ;
b_obj = obj ; b_kstr = obj -> NameInternal -> size [ 0 ] * obj ->
NameInternal -> size [ 1 ] ; obj -> NameInternal -> size [ 0 ] = 1 ; obj ->
NameInternal -> size [ 1 ] = 5 ; e3n3mnwk4b ( obj -> NameInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { obj -> NameInternal -> data [
b_kstr ] = tmp [ b_kstr ] ; } obj -> ParentIndex = 4.0 ; b_kstr = obj ->
JointInternal . Type -> size [ 0 ] * obj -> JointInternal . Type -> size [ 1
] ; obj -> JointInternal . Type -> size [ 0 ] = 1 ; obj -> JointInternal .
Type -> size [ 1 ] = 8 ; e3n3mnwk4b ( obj -> JointInternal . Type , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { obj -> JointInternal . Type
-> data [ b_kstr ] = tmp_p [ b_kstr ] ; } m2n1niwoq4 ( & switch_expression ,
2 ) ; b_kstr = switch_expression -> size [ 0 ] * switch_expression -> size [
1 ] ; switch_expression -> size [ 0 ] = 1 ; switch_expression -> size [ 1 ] =
obj -> JointInternal . Type -> size [ 1 ] ; e3n3mnwk4b ( switch_expression ,
b_kstr ) ; loop_ub = obj -> JointInternal . Type -> size [ 1 ] ; for ( b_kstr
= 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [ b_kstr ] =
obj -> JointInternal . Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr
< 8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_p [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_e [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : obj -> JointInternal .
PositionNumber = 1.0 ; obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ;
obj -> JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; case 1 : obj -> JointInternal .
PositionNumber = 1.0 ; obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ;
obj -> JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; default : obj -> JointInternal .
PositionNumber = 0.0 ; obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ;
obj -> JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 0.0 ; break ; } for ( b_kstr = 0 ; b_kstr < 16 ;
b_kstr ++ ) { obj -> JointInternal . JointToParentTransform [ b_kstr ] =
tmp_i [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj ->
JointInternal . ChildToJointTransform [ b_kstr ] = tmp_m [ b_kstr ] ; } obj
-> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 1 ] = 1.0 ; obj -> JointInternal . JointAxisInternal [ 2
] = 0.0 ; p1kacjwiri ( & obj -> CollisionsInternal ) ; obj ->
matlabCodegenIsDeleted = false ; return b_obj ; } static alyuhn31fb *
j00sarju5h ( alyuhn31fb * obj ) { alyuhn31fb * b_obj ; e1n154mvbh *
switch_expression ; int32_T b_kstr ; int32_T loop_ub ; char_T b_p [ 9 ] ;
char_T b [ 8 ] ; boolean_T b_bool ; static const char_T tmp [ 17 ] = { 'e' ,
'n' , 'd' , '_' , 'e' , 'f' , 'f' , 'e' , 'c' , 't' , 'o' , 'r' , '_' , 'l' ,
'i' , 'n' , 'k' } ; static const char_T tmp_p [ 5 ] = { 'f' , 'i' , 'x' , 'e'
, 'd' } ; static const char_T tmp_e [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l' ,
'u' , 't' , 'e' } ; static const char_T tmp_i [ 9 ] = { 'p' , 'r' , 'i' , 's'
, 'm' , 'a' , 't' , 'i' , 'c' } ; static const real_T tmp_m [ 16 ] = { 1.0 ,
0.0 , - 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.13 ,
0.0 , 0.0 , 1.0 } ; static const real_T tmp_g [ 16 ] = { 1.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 }
; int32_T exitg1 ; b_obj = obj ; b_kstr = obj -> NameInternal -> size [ 0 ] *
obj -> NameInternal -> size [ 1 ] ; obj -> NameInternal -> size [ 0 ] = 1 ;
obj -> NameInternal -> size [ 1 ] = 17 ; e3n3mnwk4b ( obj -> NameInternal ,
b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 17 ; b_kstr ++ ) { obj -> NameInternal
-> data [ b_kstr ] = tmp [ b_kstr ] ; } obj -> ParentIndex = 5.0 ; b_kstr =
obj -> JointInternal . Type -> size [ 0 ] * obj -> JointInternal . Type ->
size [ 1 ] ; obj -> JointInternal . Type -> size [ 0 ] = 1 ; obj ->
JointInternal . Type -> size [ 1 ] = 5 ; e3n3mnwk4b ( obj -> JointInternal .
Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { obj ->
JointInternal . Type -> data [ b_kstr ] = tmp_p [ b_kstr ] ; } m2n1niwoq4 ( &
switch_expression , 2 ) ; b_kstr = switch_expression -> size [ 0 ] *
switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = obj -> JointInternal . Type -> size [ 1 ] ;
e3n3mnwk4b ( switch_expression , b_kstr ) ; loop_ub = obj -> JointInternal .
Type -> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) {
switch_expression -> data [ b_kstr ] = obj -> JointInternal . Type -> data [
b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { b [ b_kstr ] =
tmp_e [ b_kstr ] ; } b_bool = false ; if ( switch_expression -> size [ 1 ] !=
8 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if (
switch_expression -> data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1 ;
} else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0 ; } else { for ( b_kstr = 0 ;
b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_i [ b_kstr ] ; } if (
switch_expression -> size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1
; } else { b_kstr = - 1 ; } } fagcrr3avn ( & switch_expression ) ; switch (
b_kstr ) { case 0 : obj -> JointInternal . PositionNumber = 1.0 ; obj ->
JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2
] = 1.0 ; break ; case 1 : obj -> JointInternal . PositionNumber = 1.0 ; obj
-> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2
] = 1.0 ; break ; default : obj -> JointInternal . PositionNumber = 0.0 ; obj
-> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2
] = 0.0 ; break ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj ->
JointInternal . JointToParentTransform [ b_kstr ] = tmp_m [ b_kstr ] ; } for
( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj -> JointInternal .
ChildToJointTransform [ b_kstr ] = tmp_g [ b_kstr ] ; } obj -> JointInternal
. JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [
1 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 0.0 ; p1kacjwiri
( & obj -> CollisionsInternal ) ; obj -> matlabCodegenIsDeleted = false ;
return b_obj ; } static alyuhn31fb * bgbooan2dw ( alyuhn31fb * obj ) {
alyuhn31fb * b_obj ; e1n154mvbh * switch_expression ; int32_T b_kstr ;
int32_T loop_ub ; char_T b_p [ 9 ] ; char_T b [ 8 ] ; boolean_T b_bool ;
static const char_T tmp [ 12 ] = { 'g' , 'r' , 'i' , 'p' , 'p' , 'e' , 'r' ,
'_' , 'l' , 'i' , 'n' , 'k' } ; static const char_T tmp_p [ 9 ] = { 'p' , 'r'
, 'i' , 's' , 'm' , 'a' , 't' , 'i' , 'c' } ; static const char_T tmp_e [ 8 ]
= { 'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ; static const real_T
tmp_i [ 16 ] = { 1.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0
, 1.0 , 0.0 , 0.0817 , 0.019 , 0.0 , 1.0 } ; static const real_T tmp_m [ 16 ]
= { 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 ,
0.0 , 0.0 , 0.0 , 1.0 } ; int32_T exitg1 ; b_obj = obj ; b_kstr = obj ->
NameInternal -> size [ 0 ] * obj -> NameInternal -> size [ 1 ] ; obj ->
NameInternal -> size [ 0 ] = 1 ; obj -> NameInternal -> size [ 1 ] = 12 ;
e3n3mnwk4b ( obj -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 12
; b_kstr ++ ) { obj -> NameInternal -> data [ b_kstr ] = tmp [ b_kstr ] ; }
obj -> ParentIndex = 5.0 ; b_kstr = obj -> JointInternal . Type -> size [ 0 ]
* obj -> JointInternal . Type -> size [ 1 ] ; obj -> JointInternal . Type ->
size [ 0 ] = 1 ; obj -> JointInternal . Type -> size [ 1 ] = 9 ; e3n3mnwk4b (
obj -> JointInternal . Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 9 ;
b_kstr ++ ) { obj -> JointInternal . Type -> data [ b_kstr ] = tmp_p [ b_kstr
] ; } m2n1niwoq4 ( & switch_expression , 2 ) ; b_kstr = switch_expression ->
size [ 0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0
] = 1 ; switch_expression -> size [ 1 ] = obj -> JointInternal . Type -> size
[ 1 ] ; e3n3mnwk4b ( switch_expression , b_kstr ) ; loop_ub = obj ->
JointInternal . Type -> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ;
b_kstr ++ ) { switch_expression -> data [ b_kstr ] = obj -> JointInternal .
Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { b [
b_kstr ] = tmp_e [ b_kstr ] ; } b_bool = false ; if ( switch_expression ->
size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 <
8 ) { if ( switch_expression -> data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) {
exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } }
while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0 ; } else { for ( b_kstr
= 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_p [ b_kstr ] ; } if (
switch_expression -> size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1
; } else { b_kstr = - 1 ; } } fagcrr3avn ( & switch_expression ) ; switch (
b_kstr ) { case 0 : obj -> JointInternal . PositionNumber = 1.0 ; obj ->
JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2
] = 1.0 ; break ; case 1 : obj -> JointInternal . PositionNumber = 1.0 ; obj
-> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2
] = 1.0 ; break ; default : obj -> JointInternal . PositionNumber = 0.0 ; obj
-> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2
] = 0.0 ; break ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj ->
JointInternal . JointToParentTransform [ b_kstr ] = tmp_i [ b_kstr ] ; } for
( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj -> JointInternal .
ChildToJointTransform [ b_kstr ] = tmp_m [ b_kstr ] ; } obj -> JointInternal
. JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [
1 ] = 1.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 0.0 ; p1kacjwiri
( & obj -> CollisionsInternal ) ; obj -> matlabCodegenIsDeleted = false ;
return b_obj ; } static alyuhn31fb * g22snpgfll ( alyuhn31fb * obj ) {
alyuhn31fb * b_obj ; e1n154mvbh * switch_expression ; int32_T b_kstr ;
int32_T loop_ub ; char_T b_p [ 9 ] ; char_T b [ 8 ] ; boolean_T b_bool ;
static const char_T tmp [ 16 ] = { 'g' , 'r' , 'i' , 'p' , 'p' , 'e' , 'r' ,
'_' , 'l' , 'i' , 'n' , 'k' , '_' , 's' , 'u' , 'b' } ; static const char_T
tmp_p [ 9 ] = { 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' , 'i' , 'c' } ;
static const char_T tmp_e [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't' ,
'e' } ; static const real_T tmp_i [ 16 ] = { 1.0 , 0.0 , - 0.0 , 0.0 , 0.0 ,
1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0817 , - 0.019 , 0.0 , 1.0 } ;
static const real_T tmp_m [ 16 ] = { 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0
, 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 } ; int32_T exitg1 ;
b_obj = obj ; b_kstr = obj -> NameInternal -> size [ 0 ] * obj ->
NameInternal -> size [ 1 ] ; obj -> NameInternal -> size [ 0 ] = 1 ; obj ->
NameInternal -> size [ 1 ] = 16 ; e3n3mnwk4b ( obj -> NameInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj -> NameInternal -> data
[ b_kstr ] = tmp [ b_kstr ] ; } obj -> ParentIndex = 5.0 ; b_kstr = obj ->
JointInternal . Type -> size [ 0 ] * obj -> JointInternal . Type -> size [ 1
] ; obj -> JointInternal . Type -> size [ 0 ] = 1 ; obj -> JointInternal .
Type -> size [ 1 ] = 9 ; e3n3mnwk4b ( obj -> JointInternal . Type , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { obj -> JointInternal . Type
-> data [ b_kstr ] = tmp_p [ b_kstr ] ; } m2n1niwoq4 ( & switch_expression ,
2 ) ; b_kstr = switch_expression -> size [ 0 ] * switch_expression -> size [
1 ] ; switch_expression -> size [ 0 ] = 1 ; switch_expression -> size [ 1 ] =
obj -> JointInternal . Type -> size [ 1 ] ; e3n3mnwk4b ( switch_expression ,
b_kstr ) ; loop_ub = obj -> JointInternal . Type -> size [ 1 ] ; for ( b_kstr
= 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [ b_kstr ] =
obj -> JointInternal . Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr
< 8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_e [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_p [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : obj -> JointInternal .
PositionNumber = 1.0 ; obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ;
obj -> JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; case 1 : obj -> JointInternal .
PositionNumber = 1.0 ; obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ;
obj -> JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; default : obj -> JointInternal .
PositionNumber = 0.0 ; obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ;
obj -> JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 0.0 ; break ; } for ( b_kstr = 0 ; b_kstr < 16 ;
b_kstr ++ ) { obj -> JointInternal . JointToParentTransform [ b_kstr ] =
tmp_i [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj ->
JointInternal . ChildToJointTransform [ b_kstr ] = tmp_m [ b_kstr ] ; } obj
-> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 1 ] = - 1.0 ; obj -> JointInternal . JointAxisInternal [
2 ] = 0.0 ; p1kacjwiri ( & obj -> CollisionsInternal ) ; obj ->
matlabCodegenIsDeleted = false ; return b_obj ; } static alyuhn31fb *
o0s2nkqu2g ( alyuhn31fb * obj ) { alyuhn31fb * b_obj ; e1n154mvbh *
switch_expression ; int32_T b_kstr ; int32_T loop_ub ; char_T b_p [ 9 ] ;
char_T b [ 8 ] ; boolean_T b_bool ; static const char_T tmp [ 12 ] = { 'e' ,
'n' , 'd' , '_' , 'e' , 'f' , 'f' , 'e' , 'c' , 't' , 'o' , 'r' } ; static
const char_T tmp_p [ 5 ] = { 'f' , 'i' , 'x' , 'e' , 'd' } ; static const
char_T tmp_e [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ;
static const char_T tmp_i [ 9 ] = { 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' ,
'i' , 'c' } ; static const real_T tmp_m [ 16 ] = { 1.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.12 , 0.0 , 0.0 , 1.0 } ;
static const real_T tmp_g [ 16 ] = { 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0
, 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 } ; int32_T exitg1 ;
b_obj = obj ; b_kstr = obj -> NameInternal -> size [ 0 ] * obj ->
NameInternal -> size [ 1 ] ; obj -> NameInternal -> size [ 0 ] = 1 ; obj ->
NameInternal -> size [ 1 ] = 12 ; e3n3mnwk4b ( obj -> NameInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 12 ; b_kstr ++ ) { obj -> NameInternal -> data
[ b_kstr ] = tmp [ b_kstr ] ; } obj -> ParentIndex = 5.0 ; b_kstr = obj ->
JointInternal . Type -> size [ 0 ] * obj -> JointInternal . Type -> size [ 1
] ; obj -> JointInternal . Type -> size [ 0 ] = 1 ; obj -> JointInternal .
Type -> size [ 1 ] = 5 ; e3n3mnwk4b ( obj -> JointInternal . Type , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { obj -> JointInternal . Type
-> data [ b_kstr ] = tmp_p [ b_kstr ] ; } m2n1niwoq4 ( & switch_expression ,
2 ) ; b_kstr = switch_expression -> size [ 0 ] * switch_expression -> size [
1 ] ; switch_expression -> size [ 0 ] = 1 ; switch_expression -> size [ 1 ] =
obj -> JointInternal . Type -> size [ 1 ] ; e3n3mnwk4b ( switch_expression ,
b_kstr ) ; loop_ub = obj -> JointInternal . Type -> size [ 1 ] ; for ( b_kstr
= 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [ b_kstr ] =
obj -> JointInternal . Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr
< 8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_e [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_i [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : obj -> JointInternal .
PositionNumber = 1.0 ; obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ;
obj -> JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; case 1 : obj -> JointInternal .
PositionNumber = 1.0 ; obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ;
obj -> JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; default : obj -> JointInternal .
PositionNumber = 0.0 ; obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ;
obj -> JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 0.0 ; break ; } for ( b_kstr = 0 ; b_kstr < 16 ;
b_kstr ++ ) { obj -> JointInternal . JointToParentTransform [ b_kstr ] =
tmp_m [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj ->
JointInternal . ChildToJointTransform [ b_kstr ] = tmp_g [ b_kstr ] ; } obj
-> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2
] = 0.0 ; p1kacjwiri ( & obj -> CollisionsInternal ) ; obj ->
matlabCodegenIsDeleted = false ; return b_obj ; } static void ep2455cw5m (
evzhsot2uw * obj ) { e1n154mvbh * switch_expression ; mjv2tzlyyi * obj_p ;
int32_T b_kstr ; int32_T loop_ub ; char_T b_p [ 9 ] ; char_T b [ 8 ] ;
boolean_T b_bool ; static const char_T tmp [ 5 ] = { 'l' , 'i' , 'n' , 'k' ,
'1' } ; static const char_T tmp_p [ 5 ] = { 'f' , 'i' , 'x' , 'e' , 'd' } ;
static const char_T tmp_e [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't' ,
'e' } ; static const char_T tmp_i [ 9 ] = { 'p' , 'r' , 'i' , 's' , 'm' , 'a'
, 't' , 'i' , 'c' } ; static const real_T tmp_m [ 16 ] = { 1.0 , 0.0 , - 0.0
, 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0
} ; static const real_T tmp_g [ 16 ] = { 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 } ; static const
char_T tmp_j [ 5 ] = { 'l' , 'i' , 'n' , 'k' , '2' } ; static const real_T
tmp_f [ 16 ] = { 1.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0
, 1.0 , 0.0 , 0.012 , 0.0 , 0.017 , 1.0 } ; static const char_T tmp_c [ 5 ] =
{ 'l' , 'i' , 'n' , 'k' , '3' } ; static const real_T tmp_k [ 16 ] = { 1.0 ,
0.0 , - 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0
, 0.058 , 1.0 } ; static const char_T tmp_b [ 5 ] = { 'w' , 'o' , 'r' , 'l' ,
'd' } ; int32_T exitg1 ; obj -> isInitialized = 1 ; obj_p = & obj ->
TreeInternal ; obj -> TreeInternal . NumBodies = 9.0 ; kff4ccpwjv ( & obj ->
TreeInternal , & obj -> TreeInternal . _pobj0 [ 0 ] ) ; b_kstr = obj_p ->
_pobj0 [ 9 ] . NameInternal -> size [ 0 ] * obj_p -> _pobj0 [ 9 ] .
NameInternal -> size [ 1 ] ; obj_p -> _pobj0 [ 9 ] . NameInternal -> size [ 0
] = 1 ; obj_p -> _pobj0 [ 9 ] . NameInternal -> size [ 1 ] = 5 ; e3n3mnwk4b (
obj_p -> _pobj0 [ 9 ] . NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr <
5 ; b_kstr ++ ) { obj_p -> _pobj0 [ 9 ] . NameInternal -> data [ b_kstr ] =
tmp [ b_kstr ] ; } obj_p -> _pobj0 [ 9 ] . ParentIndex = 0.0 ; b_kstr = obj
-> TreeInternal . _pobj0 [ 9 ] . JointInternal . Type -> size [ 0 ] * obj ->
TreeInternal . _pobj0 [ 9 ] . JointInternal . Type -> size [ 1 ] ; obj ->
TreeInternal . _pobj0 [ 9 ] . JointInternal . Type -> size [ 0 ] = 1 ; obj ->
TreeInternal . _pobj0 [ 9 ] . JointInternal . Type -> size [ 1 ] = 5 ;
e3n3mnwk4b ( obj -> TreeInternal . _pobj0 [ 9 ] . JointInternal . Type ,
b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { obj -> TreeInternal
. _pobj0 [ 9 ] . JointInternal . Type -> data [ b_kstr ] = tmp_p [ b_kstr ] ;
} m2n1niwoq4 ( & switch_expression , 2 ) ; b_kstr = switch_expression -> size
[ 0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1
; switch_expression -> size [ 1 ] = obj -> TreeInternal . _pobj0 [ 9 ] .
JointInternal . Type -> size [ 1 ] ; e3n3mnwk4b ( switch_expression , b_kstr
) ; loop_ub = obj -> TreeInternal . _pobj0 [ 9 ] . JointInternal . Type ->
size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) {
switch_expression -> data [ b_kstr ] = obj -> TreeInternal . _pobj0 [ 9 ] .
JointInternal . Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 8 ;
b_kstr ++ ) { b [ b_kstr ] = tmp_e [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_i [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } switch ( b_kstr ) {
case 0 : obj -> TreeInternal . _pobj0 [ 9 ] . JointInternal . PositionNumber
= 1.0 ; obj -> TreeInternal . _pobj0 [ 9 ] . JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> TreeInternal . _pobj0 [ 9 ] .
JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> TreeInternal . _pobj0
[ 9 ] . JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ; case 1 : obj
-> TreeInternal . _pobj0 [ 9 ] . JointInternal . PositionNumber = 1.0 ; obj
-> TreeInternal . _pobj0 [ 9 ] . JointInternal . JointAxisInternal [ 0 ] =
0.0 ; obj -> TreeInternal . _pobj0 [ 9 ] . JointInternal . JointAxisInternal
[ 1 ] = 0.0 ; obj -> TreeInternal . _pobj0 [ 9 ] . JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; default : obj -> TreeInternal .
_pobj0 [ 9 ] . JointInternal . PositionNumber = 0.0 ; obj -> TreeInternal .
_pobj0 [ 9 ] . JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj ->
TreeInternal . _pobj0 [ 9 ] . JointInternal . JointAxisInternal [ 1 ] = 0.0 ;
obj -> TreeInternal . _pobj0 [ 9 ] . JointInternal . JointAxisInternal [ 2 ]
= 0.0 ; break ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj_p ->
_pobj0 [ 9 ] . JointInternal . JointToParentTransform [ b_kstr ] = tmp_m [
b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj_p -> _pobj0 [
9 ] . JointInternal . ChildToJointTransform [ b_kstr ] = tmp_g [ b_kstr ] ; }
obj_p -> _pobj0 [ 9 ] . JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj_p
-> _pobj0 [ 9 ] . JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj_p ->
_pobj0 [ 9 ] . JointInternal . JointAxisInternal [ 2 ] = 0.0 ; p1kacjwiri ( &
obj_p -> _pobj0 [ 9 ] . CollisionsInternal ) ; obj_p -> _pobj0 [ 9 ] .
matlabCodegenIsDeleted = false ; obj -> TreeInternal . Bodies [ 0 ] = & obj_p
-> _pobj0 [ 9 ] ; b_kstr = obj_p -> _pobj0 [ 10 ] . NameInternal -> size [ 0
] * obj_p -> _pobj0 [ 10 ] . NameInternal -> size [ 1 ] ; obj_p -> _pobj0 [
10 ] . NameInternal -> size [ 0 ] = 1 ; obj_p -> _pobj0 [ 10 ] . NameInternal
-> size [ 1 ] = 5 ; e3n3mnwk4b ( obj_p -> _pobj0 [ 10 ] . NameInternal ,
b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { obj_p -> _pobj0 [ 10
] . NameInternal -> data [ b_kstr ] = tmp_j [ b_kstr ] ; } obj_p -> _pobj0 [
10 ] . ParentIndex = 1.0 ; b_kstr = obj -> TreeInternal . _pobj0 [ 10 ] .
JointInternal . Type -> size [ 0 ] * obj -> TreeInternal . _pobj0 [ 10 ] .
JointInternal . Type -> size [ 1 ] ; obj -> TreeInternal . _pobj0 [ 10 ] .
JointInternal . Type -> size [ 0 ] = 1 ; obj -> TreeInternal . _pobj0 [ 10 ]
. JointInternal . Type -> size [ 1 ] = 8 ; e3n3mnwk4b ( obj -> TreeInternal .
_pobj0 [ 10 ] . JointInternal . Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr <
8 ; b_kstr ++ ) { obj -> TreeInternal . _pobj0 [ 10 ] . JointInternal . Type
-> data [ b_kstr ] = tmp_e [ b_kstr ] ; } b_kstr = switch_expression -> size
[ 0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1
; switch_expression -> size [ 1 ] = obj -> TreeInternal . _pobj0 [ 10 ] .
JointInternal . Type -> size [ 1 ] ; e3n3mnwk4b ( switch_expression , b_kstr
) ; loop_ub = obj -> TreeInternal . _pobj0 [ 10 ] . JointInternal . Type ->
size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) {
switch_expression -> data [ b_kstr ] = obj -> TreeInternal . _pobj0 [ 10 ] .
JointInternal . Type -> data [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_i [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } switch ( b_kstr ) {
case 0 : obj -> TreeInternal . _pobj0 [ 10 ] . JointInternal . PositionNumber
= 1.0 ; obj -> TreeInternal . _pobj0 [ 10 ] . JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> TreeInternal . _pobj0 [ 10 ] .
JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> TreeInternal . _pobj0
[ 10 ] . JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ; case 1 : obj
-> TreeInternal . _pobj0 [ 10 ] . JointInternal . PositionNumber = 1.0 ; obj
-> TreeInternal . _pobj0 [ 10 ] . JointInternal . JointAxisInternal [ 0 ] =
0.0 ; obj -> TreeInternal . _pobj0 [ 10 ] . JointInternal . JointAxisInternal
[ 1 ] = 0.0 ; obj -> TreeInternal . _pobj0 [ 10 ] . JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; default : obj -> TreeInternal .
_pobj0 [ 10 ] . JointInternal . PositionNumber = 0.0 ; obj -> TreeInternal .
_pobj0 [ 10 ] . JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj ->
TreeInternal . _pobj0 [ 10 ] . JointInternal . JointAxisInternal [ 1 ] = 0.0
; obj -> TreeInternal . _pobj0 [ 10 ] . JointInternal . JointAxisInternal [ 2
] = 0.0 ; break ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj_p ->
_pobj0 [ 10 ] . JointInternal . JointToParentTransform [ b_kstr ] = tmp_f [
b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj_p -> _pobj0 [
10 ] . JointInternal . ChildToJointTransform [ b_kstr ] = tmp_g [ b_kstr ] ;
} obj_p -> _pobj0 [ 10 ] . JointInternal . JointAxisInternal [ 0 ] = 0.0 ;
obj_p -> _pobj0 [ 10 ] . JointInternal . JointAxisInternal [ 1 ] = 0.0 ;
obj_p -> _pobj0 [ 10 ] . JointInternal . JointAxisInternal [ 2 ] = 1.0 ;
p1kacjwiri ( & obj_p -> _pobj0 [ 10 ] . CollisionsInternal ) ; obj_p ->
_pobj0 [ 10 ] . matlabCodegenIsDeleted = false ; obj -> TreeInternal . Bodies
[ 1 ] = & obj_p -> _pobj0 [ 10 ] ; b_kstr = obj_p -> _pobj0 [ 11 ] .
NameInternal -> size [ 0 ] * obj_p -> _pobj0 [ 11 ] . NameInternal -> size [
1 ] ; obj_p -> _pobj0 [ 11 ] . NameInternal -> size [ 0 ] = 1 ; obj_p ->
_pobj0 [ 11 ] . NameInternal -> size [ 1 ] = 5 ; e3n3mnwk4b ( obj_p -> _pobj0
[ 11 ] . NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++
) { obj_p -> _pobj0 [ 11 ] . NameInternal -> data [ b_kstr ] = tmp_c [ b_kstr
] ; } obj_p -> _pobj0 [ 11 ] . ParentIndex = 2.0 ; b_kstr = obj ->
TreeInternal . _pobj0 [ 11 ] . JointInternal . Type -> size [ 0 ] * obj ->
TreeInternal . _pobj0 [ 11 ] . JointInternal . Type -> size [ 1 ] ; obj ->
TreeInternal . _pobj0 [ 11 ] . JointInternal . Type -> size [ 0 ] = 1 ; obj
-> TreeInternal . _pobj0 [ 11 ] . JointInternal . Type -> size [ 1 ] = 8 ;
e3n3mnwk4b ( obj -> TreeInternal . _pobj0 [ 11 ] . JointInternal . Type ,
b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { obj -> TreeInternal
. _pobj0 [ 11 ] . JointInternal . Type -> data [ b_kstr ] = tmp_e [ b_kstr ]
; } b_kstr = switch_expression -> size [ 0 ] * switch_expression -> size [ 1
] ; switch_expression -> size [ 0 ] = 1 ; switch_expression -> size [ 1 ] =
obj -> TreeInternal . _pobj0 [ 11 ] . JointInternal . Type -> size [ 1 ] ;
e3n3mnwk4b ( switch_expression , b_kstr ) ; loop_ub = obj -> TreeInternal .
_pobj0 [ 11 ] . JointInternal . Type -> size [ 1 ] ; for ( b_kstr = 0 ;
b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [ b_kstr ] = obj
-> TreeInternal . _pobj0 [ 11 ] . JointInternal . Type -> data [ b_kstr ] ; }
b_bool = false ; if ( switch_expression -> size [ 1 ] != 8 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 0 ; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++
) { b_p [ b_kstr ] = tmp_i [ b_kstr ] ; } if ( switch_expression -> size [ 1
] != 9 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if
( switch_expression -> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 =
1 ; } else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } }
switch ( b_kstr ) { case 0 : obj -> TreeInternal . _pobj0 [ 11 ] .
JointInternal . PositionNumber = 1.0 ; obj -> TreeInternal . _pobj0 [ 11 ] .
JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj -> TreeInternal . _pobj0
[ 11 ] . JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> TreeInternal
. _pobj0 [ 11 ] . JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ;
case 1 : obj -> TreeInternal . _pobj0 [ 11 ] . JointInternal . PositionNumber
= 1.0 ; obj -> TreeInternal . _pobj0 [ 11 ] . JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> TreeInternal . _pobj0 [ 11 ] .
JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> TreeInternal . _pobj0
[ 11 ] . JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ; default :
obj -> TreeInternal . _pobj0 [ 11 ] . JointInternal . PositionNumber = 0.0 ;
obj -> TreeInternal . _pobj0 [ 11 ] . JointInternal . JointAxisInternal [ 0 ]
= 0.0 ; obj -> TreeInternal . _pobj0 [ 11 ] . JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; obj -> TreeInternal . _pobj0 [ 11 ] .
JointInternal . JointAxisInternal [ 2 ] = 0.0 ; break ; } for ( b_kstr = 0 ;
b_kstr < 16 ; b_kstr ++ ) { obj_p -> _pobj0 [ 11 ] . JointInternal .
JointToParentTransform [ b_kstr ] = tmp_k [ b_kstr ] ; } for ( b_kstr = 0 ;
b_kstr < 16 ; b_kstr ++ ) { obj_p -> _pobj0 [ 11 ] . JointInternal .
ChildToJointTransform [ b_kstr ] = tmp_g [ b_kstr ] ; } obj_p -> _pobj0 [ 11
] . JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj_p -> _pobj0 [ 11 ] .
JointInternal . JointAxisInternal [ 1 ] = 1.0 ; obj_p -> _pobj0 [ 11 ] .
JointInternal . JointAxisInternal [ 2 ] = 0.0 ; p1kacjwiri ( & obj_p ->
_pobj0 [ 11 ] . CollisionsInternal ) ; obj_p -> _pobj0 [ 11 ] .
matlabCodegenIsDeleted = false ; obj -> TreeInternal . Bodies [ 2 ] = & obj_p
-> _pobj0 [ 11 ] ; obj -> TreeInternal . Bodies [ 3 ] = nnpwrpxrzkqixzq ( &
obj -> TreeInternal . _pobj0 [ 12 ] ) ; obj -> TreeInternal . Bodies [ 4 ] =
b4tj0yrbq0 ( & obj -> TreeInternal . _pobj0 [ 13 ] ) ; obj -> TreeInternal .
Bodies [ 5 ] = j00sarju5h ( & obj -> TreeInternal . _pobj0 [ 14 ] ) ; obj ->
TreeInternal . Bodies [ 6 ] = bgbooan2dw ( & obj -> TreeInternal . _pobj0 [
15 ] ) ; obj -> TreeInternal . Bodies [ 7 ] = g22snpgfll ( & obj ->
TreeInternal . _pobj0 [ 16 ] ) ; obj -> TreeInternal . Bodies [ 8 ] =
o0s2nkqu2g ( & obj -> TreeInternal . _pobj0 [ 17 ] ) ; obj -> TreeInternal .
PositionNumber = 6.0 ; b_kstr = obj_p -> Base . NameInternal -> size [ 0 ] *
obj_p -> Base . NameInternal -> size [ 1 ] ; obj_p -> Base . NameInternal ->
size [ 0 ] = 1 ; obj_p -> Base . NameInternal -> size [ 1 ] = 5 ; e3n3mnwk4b
( obj_p -> Base . NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ;
b_kstr ++ ) { obj_p -> Base . NameInternal -> data [ b_kstr ] = tmp_b [
b_kstr ] ; } obj_p -> Base . ParentIndex = - 1.0 ; b_kstr = obj ->
TreeInternal . Base . JointInternal . Type -> size [ 0 ] * obj ->
TreeInternal . Base . JointInternal . Type -> size [ 1 ] ; obj ->
TreeInternal . Base . JointInternal . Type -> size [ 0 ] = 1 ; obj ->
TreeInternal . Base . JointInternal . Type -> size [ 1 ] = 5 ; e3n3mnwk4b (
obj -> TreeInternal . Base . JointInternal . Type , b_kstr ) ; for ( b_kstr =
0 ; b_kstr < 5 ; b_kstr ++ ) { obj -> TreeInternal . Base . JointInternal .
Type -> data [ b_kstr ] = tmp_p [ b_kstr ] ; } b_kstr = switch_expression ->
size [ 0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0
] = 1 ; switch_expression -> size [ 1 ] = obj -> TreeInternal . Base .
JointInternal . Type -> size [ 1 ] ; e3n3mnwk4b ( switch_expression , b_kstr
) ; loop_ub = obj -> TreeInternal . Base . JointInternal . Type -> size [ 1 ]
; for ( b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression ->
data [ b_kstr ] = obj -> TreeInternal . Base . JointInternal . Type -> data [
b_kstr ] ; } b_bool = false ; if ( switch_expression -> size [ 1 ] != 8 ) { }
else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if (
switch_expression -> data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1 ;
} else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0 ; } else { for ( b_kstr = 0 ;
b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_i [ b_kstr ] ; } if (
switch_expression -> size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1
; } else { b_kstr = - 1 ; } } fagcrr3avn ( & switch_expression ) ; switch (
b_kstr ) { case 0 : obj -> TreeInternal . Base . JointInternal .
PositionNumber = 1.0 ; obj -> TreeInternal . Base . JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> TreeInternal . Base . JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; obj -> TreeInternal . Base . JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; case 1 : obj -> TreeInternal . Base .
JointInternal . PositionNumber = 1.0 ; obj -> TreeInternal . Base .
JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj -> TreeInternal . Base .
JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> TreeInternal . Base .
JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ; default : obj ->
TreeInternal . Base . JointInternal . PositionNumber = 0.0 ; obj ->
TreeInternal . Base . JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj ->
TreeInternal . Base . JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj ->
TreeInternal . Base . JointInternal . JointAxisInternal [ 2 ] = 0.0 ; break ;
} p1kacjwiri ( & obj_p -> Base . CollisionsInternal ) ; obj_p -> Base .
matlabCodegenIsDeleted = false ; obj -> TreeInternal . matlabCodegenIsDeleted
= false ; } static void fx50peix55 ( ot4uovzkt0wp * pStruct ) { m2n1niwoq4 (
& pStruct -> Type , 2 ) ; calykgtvmm ( & pStruct -> MotionSubspace , 2 ) ; }
static void b4adihsbvg ( alyuhn31fbf * pStruct ) { m2n1niwoq4 ( & pStruct ->
NameInternal , 2 ) ; fx50peix55 ( & pStruct -> JointInternal ) ; iwumdx5q3w (
& pStruct -> CollisionsInternal ) ; } static void ezyujupqoplefpe (
alyuhn31fbf pMatrix [ 18 ] ) { int32_T i ; for ( i = 0 ; i < 18 ; i ++ ) {
b4adihsbvg ( & pMatrix [ i ] ) ; } } static void hzaeefljc4 ( mjv2tzlyyig *
pStruct ) { b4adihsbvg ( & pStruct -> Base ) ; ezyujupqoplefpe ( pStruct ->
_pobj0 ) ; } static void ecqu5ikgmiel ( jd4q4eqa1g * pStruct ) { hzaeefljc4 (
& pStruct -> TreeInternal ) ; } static alyuhn31fbf * f502siufoq ( alyuhn31fbf
* obj ) { alyuhn31fbf * b_obj ; e1n154mvbh * switch_expression ; int32_T
b_kstr ; int32_T loop_ub ; char_T b_p [ 9 ] ; char_T b [ 8 ] ; int8_T
msubspace_data [ 36 ] ; int8_T tmp [ 6 ] ; boolean_T b_bool ; static const
char_T tmp_p [ 10 ] = { 'd' , 'u' , 'm' , 'm' , 'y' , 'b' , 'o' , 'd' , 'y' ,
'5' } ; static const char_T tmp_e [ 5 ] = { 'f' , 'i' , 'x' , 'e' , 'd' } ;
static const char_T tmp_i [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't' ,
'e' } ; static const char_T tmp_m [ 9 ] = { 'p' , 'r' , 'i' , 's' , 'm' , 'a'
, 't' , 'i' , 'c' } ; int32_T exitg1 ; b_obj = obj ; b_kstr = obj ->
NameInternal -> size [ 0 ] * obj -> NameInternal -> size [ 1 ] ; obj ->
NameInternal -> size [ 0 ] = 1 ; obj -> NameInternal -> size [ 1 ] = 10 ;
e3n3mnwk4b ( obj -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 10
; b_kstr ++ ) { obj -> NameInternal -> data [ b_kstr ] = tmp_p [ b_kstr ] ; }
b_kstr = obj -> JointInternal . Type -> size [ 0 ] * obj -> JointInternal .
Type -> size [ 1 ] ; obj -> JointInternal . Type -> size [ 0 ] = 1 ; obj ->
JointInternal . Type -> size [ 1 ] = 5 ; e3n3mnwk4b ( obj -> JointInternal .
Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { obj ->
JointInternal . Type -> data [ b_kstr ] = tmp_e [ b_kstr ] ; } m2n1niwoq4 ( &
switch_expression , 2 ) ; b_kstr = switch_expression -> size [ 0 ] *
switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = obj -> JointInternal . Type -> size [ 1 ] ;
e3n3mnwk4b ( switch_expression , b_kstr ) ; loop_ub = obj -> JointInternal .
Type -> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) {
switch_expression -> data [ b_kstr ] = obj -> JointInternal . Type -> data [
b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { b [ b_kstr ] =
tmp_i [ b_kstr ] ; } b_bool = false ; if ( switch_expression -> size [ 1 ] !=
8 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if (
switch_expression -> data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1 ;
} else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0 ; } else { for ( b_kstr = 0 ;
b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_m [ b_kstr ] ; } if (
switch_expression -> size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1
; } else { b_kstr = - 1 ; } } fagcrr3avn ( & switch_expression ) ; switch (
b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ]
= 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr
++ ) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } obj -> JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 1
] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ; case
1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] = 0 ; tmp [ 4 ]
= 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } obj -> JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 1
] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ;
default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [
b_kstr ] = 0 ; } obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj
-> JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = obj -> JointInternal .
MotionSubspace -> size [ 0 ] * obj -> JointInternal . MotionSubspace -> size
[ 1 ] ; obj -> JointInternal . MotionSubspace -> size [ 0 ] = 6 ; obj ->
JointInternal . MotionSubspace -> size [ 1 ] = 1 ; miyl1rq4uj ( obj ->
JointInternal . MotionSubspace , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 6 ;
b_kstr ++ ) { obj -> JointInternal . MotionSubspace -> data [ b_kstr ] =
msubspace_data [ b_kstr ] ; } obj -> Index = - 1.0 ; obj -> ParentIndex = -
1.0 ; for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) { msubspace_data [ b_kstr
] = 0 ; } for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [
b_kstr + 6 * b_kstr ] = 1 ; } for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) {
obj -> SpatialInertia [ b_kstr ] = msubspace_data [ b_kstr ] ; } p1kacjwiri (
& obj -> CollisionsInternal ) ; obj -> matlabCodegenIsDeleted = false ;
return b_obj ; } static alyuhn31fbf * au3yg4mglc ( alyuhn31fbf * obj ) {
alyuhn31fbf * b_obj ; e1n154mvbh * switch_expression ; int32_T b_kstr ;
int32_T loop_ub ; char_T b_p [ 9 ] ; char_T b [ 8 ] ; int8_T msubspace_data [
36 ] ; int8_T tmp [ 6 ] ; boolean_T b_bool ; static const char_T tmp_p [ 10 ]
= { 'd' , 'u' , 'm' , 'm' , 'y' , 'b' , 'o' , 'd' , 'y' , '6' } ; static
const char_T tmp_e [ 5 ] = { 'f' , 'i' , 'x' , 'e' , 'd' } ; static const
char_T tmp_i [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ;
static const char_T tmp_m [ 9 ] = { 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' ,
'i' , 'c' } ; int32_T exitg1 ; b_obj = obj ; b_kstr = obj -> NameInternal ->
size [ 0 ] * obj -> NameInternal -> size [ 1 ] ; obj -> NameInternal -> size
[ 0 ] = 1 ; obj -> NameInternal -> size [ 1 ] = 10 ; e3n3mnwk4b ( obj ->
NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 10 ; b_kstr ++ ) { obj
-> NameInternal -> data [ b_kstr ] = tmp_p [ b_kstr ] ; } b_kstr = obj ->
JointInternal . Type -> size [ 0 ] * obj -> JointInternal . Type -> size [ 1
] ; obj -> JointInternal . Type -> size [ 0 ] = 1 ; obj -> JointInternal .
Type -> size [ 1 ] = 5 ; e3n3mnwk4b ( obj -> JointInternal . Type , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { obj -> JointInternal . Type
-> data [ b_kstr ] = tmp_e [ b_kstr ] ; } m2n1niwoq4 ( & switch_expression ,
2 ) ; b_kstr = switch_expression -> size [ 0 ] * switch_expression -> size [
1 ] ; switch_expression -> size [ 0 ] = 1 ; switch_expression -> size [ 1 ] =
obj -> JointInternal . Type -> size [ 1 ] ; e3n3mnwk4b ( switch_expression ,
b_kstr ) ; loop_ub = obj -> JointInternal . Type -> size [ 1 ] ; for ( b_kstr
= 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [ b_kstr ] =
obj -> JointInternal . Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr
< 8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_i [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_m [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ]
= 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj ->
JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; case 1 : tmp [ 0 ] = 0 ; tmp [ 1 ] =
0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 1 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj ->
JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; default : for ( b_kstr = 0 ; b_kstr <
6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = 0 ; } obj -> JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 1
] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 0.0 ; break ; }
b_kstr = obj -> JointInternal . MotionSubspace -> size [ 0 ] * obj ->
JointInternal . MotionSubspace -> size [ 1 ] ; obj -> JointInternal .
MotionSubspace -> size [ 0 ] = 6 ; obj -> JointInternal . MotionSubspace ->
size [ 1 ] = 1 ; miyl1rq4uj ( obj -> JointInternal . MotionSubspace , b_kstr
) ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { obj -> JointInternal .
MotionSubspace -> data [ b_kstr ] = msubspace_data [ b_kstr ] ; } obj ->
Index = - 1.0 ; obj -> ParentIndex = - 1.0 ; for ( b_kstr = 0 ; b_kstr < 36 ;
b_kstr ++ ) { msubspace_data [ b_kstr ] = 0 ; } for ( b_kstr = 0 ; b_kstr < 6
; b_kstr ++ ) { msubspace_data [ b_kstr + 6 * b_kstr ] = 1 ; } for ( b_kstr =
0 ; b_kstr < 36 ; b_kstr ++ ) { obj -> SpatialInertia [ b_kstr ] =
msubspace_data [ b_kstr ] ; } p1kacjwiri ( & obj -> CollisionsInternal ) ;
obj -> matlabCodegenIsDeleted = false ; return b_obj ; } static alyuhn31fbf *
ir0wvh3inp ( alyuhn31fbf * obj ) { alyuhn31fbf * b_obj ; e1n154mvbh *
switch_expression ; int32_T b_kstr ; int32_T loop_ub ; char_T b_p [ 9 ] ;
char_T b [ 8 ] ; int8_T msubspace_data [ 36 ] ; int8_T tmp [ 6 ] ; boolean_T
b_bool ; static const char_T tmp_p [ 10 ] = { 'd' , 'u' , 'm' , 'm' , 'y' ,
'b' , 'o' , 'd' , 'y' , '7' } ; static const char_T tmp_e [ 5 ] = { 'f' , 'i'
, 'x' , 'e' , 'd' } ; static const char_T tmp_i [ 8 ] = { 'r' , 'e' , 'v' ,
'o' , 'l' , 'u' , 't' , 'e' } ; static const char_T tmp_m [ 9 ] = { 'p' , 'r'
, 'i' , 's' , 'm' , 'a' , 't' , 'i' , 'c' } ; int32_T exitg1 ; b_obj = obj ;
b_kstr = obj -> NameInternal -> size [ 0 ] * obj -> NameInternal -> size [ 1
] ; obj -> NameInternal -> size [ 0 ] = 1 ; obj -> NameInternal -> size [ 1 ]
= 10 ; e3n3mnwk4b ( obj -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ;
b_kstr < 10 ; b_kstr ++ ) { obj -> NameInternal -> data [ b_kstr ] = tmp_p [
b_kstr ] ; } b_kstr = obj -> JointInternal . Type -> size [ 0 ] * obj ->
JointInternal . Type -> size [ 1 ] ; obj -> JointInternal . Type -> size [ 0
] = 1 ; obj -> JointInternal . Type -> size [ 1 ] = 5 ; e3n3mnwk4b ( obj ->
JointInternal . Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ )
{ obj -> JointInternal . Type -> data [ b_kstr ] = tmp_e [ b_kstr ] ; }
m2n1niwoq4 ( & switch_expression , 2 ) ; b_kstr = switch_expression -> size [
0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = obj -> JointInternal . Type -> size [ 1 ] ;
e3n3mnwk4b ( switch_expression , b_kstr ) ; loop_ub = obj -> JointInternal .
Type -> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) {
switch_expression -> data [ b_kstr ] = obj -> JointInternal . Type -> data [
b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { b [ b_kstr ] =
tmp_i [ b_kstr ] ; } b_bool = false ; if ( switch_expression -> size [ 1 ] !=
8 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if (
switch_expression -> data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1 ;
} else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0 ; } else { for ( b_kstr = 0 ;
b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_m [ b_kstr ] ; } if (
switch_expression -> size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1
; } else { b_kstr = - 1 ; } } fagcrr3avn ( & switch_expression ) ; switch (
b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ]
= 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr
++ ) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } obj -> JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 1
] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ; case
1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] = 0 ; tmp [ 4 ]
= 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } obj -> JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 1
] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ;
default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [
b_kstr ] = 0 ; } obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj
-> JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = obj -> JointInternal .
MotionSubspace -> size [ 0 ] * obj -> JointInternal . MotionSubspace -> size
[ 1 ] ; obj -> JointInternal . MotionSubspace -> size [ 0 ] = 6 ; obj ->
JointInternal . MotionSubspace -> size [ 1 ] = 1 ; miyl1rq4uj ( obj ->
JointInternal . MotionSubspace , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 6 ;
b_kstr ++ ) { obj -> JointInternal . MotionSubspace -> data [ b_kstr ] =
msubspace_data [ b_kstr ] ; } obj -> Index = - 1.0 ; obj -> ParentIndex = -
1.0 ; for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) { msubspace_data [ b_kstr
] = 0 ; } for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [
b_kstr + 6 * b_kstr ] = 1 ; } for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) {
obj -> SpatialInertia [ b_kstr ] = msubspace_data [ b_kstr ] ; } p1kacjwiri (
& obj -> CollisionsInternal ) ; obj -> matlabCodegenIsDeleted = false ;
return b_obj ; } static alyuhn31fbf * neqqfka14z ( alyuhn31fbf * obj ) {
alyuhn31fbf * b_obj ; e1n154mvbh * switch_expression ; int32_T b_kstr ;
int32_T loop_ub ; char_T b_p [ 9 ] ; char_T b [ 8 ] ; int8_T msubspace_data [
36 ] ; int8_T tmp [ 6 ] ; boolean_T b_bool ; static const char_T tmp_p [ 10 ]
= { 'd' , 'u' , 'm' , 'm' , 'y' , 'b' , 'o' , 'd' , 'y' , '8' } ; static
const char_T tmp_e [ 5 ] = { 'f' , 'i' , 'x' , 'e' , 'd' } ; static const
char_T tmp_i [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ;
static const char_T tmp_m [ 9 ] = { 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' ,
'i' , 'c' } ; int32_T exitg1 ; b_obj = obj ; b_kstr = obj -> NameInternal ->
size [ 0 ] * obj -> NameInternal -> size [ 1 ] ; obj -> NameInternal -> size
[ 0 ] = 1 ; obj -> NameInternal -> size [ 1 ] = 10 ; e3n3mnwk4b ( obj ->
NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 10 ; b_kstr ++ ) { obj
-> NameInternal -> data [ b_kstr ] = tmp_p [ b_kstr ] ; } b_kstr = obj ->
JointInternal . Type -> size [ 0 ] * obj -> JointInternal . Type -> size [ 1
] ; obj -> JointInternal . Type -> size [ 0 ] = 1 ; obj -> JointInternal .
Type -> size [ 1 ] = 5 ; e3n3mnwk4b ( obj -> JointInternal . Type , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { obj -> JointInternal . Type
-> data [ b_kstr ] = tmp_e [ b_kstr ] ; } m2n1niwoq4 ( & switch_expression ,
2 ) ; b_kstr = switch_expression -> size [ 0 ] * switch_expression -> size [
1 ] ; switch_expression -> size [ 0 ] = 1 ; switch_expression -> size [ 1 ] =
obj -> JointInternal . Type -> size [ 1 ] ; e3n3mnwk4b ( switch_expression ,
b_kstr ) ; loop_ub = obj -> JointInternal . Type -> size [ 1 ] ; for ( b_kstr
= 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [ b_kstr ] =
obj -> JointInternal . Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr
< 8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_i [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_m [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ]
= 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj ->
JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; case 1 : tmp [ 0 ] = 0 ; tmp [ 1 ] =
0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 1 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj ->
JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; default : for ( b_kstr = 0 ; b_kstr <
6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = 0 ; } obj -> JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 1
] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 0.0 ; break ; }
b_kstr = obj -> JointInternal . MotionSubspace -> size [ 0 ] * obj ->
JointInternal . MotionSubspace -> size [ 1 ] ; obj -> JointInternal .
MotionSubspace -> size [ 0 ] = 6 ; obj -> JointInternal . MotionSubspace ->
size [ 1 ] = 1 ; miyl1rq4uj ( obj -> JointInternal . MotionSubspace , b_kstr
) ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { obj -> JointInternal .
MotionSubspace -> data [ b_kstr ] = msubspace_data [ b_kstr ] ; } obj ->
Index = - 1.0 ; obj -> ParentIndex = - 1.0 ; for ( b_kstr = 0 ; b_kstr < 36 ;
b_kstr ++ ) { msubspace_data [ b_kstr ] = 0 ; } for ( b_kstr = 0 ; b_kstr < 6
; b_kstr ++ ) { msubspace_data [ b_kstr + 6 * b_kstr ] = 1 ; } for ( b_kstr =
0 ; b_kstr < 36 ; b_kstr ++ ) { obj -> SpatialInertia [ b_kstr ] =
msubspace_data [ b_kstr ] ; } p1kacjwiri ( & obj -> CollisionsInternal ) ;
obj -> matlabCodegenIsDeleted = false ; return b_obj ; } static alyuhn31fbf *
lafe2rmqud ( alyuhn31fbf * obj ) { alyuhn31fbf * b_obj ; e1n154mvbh *
switch_expression ; int32_T b_kstr ; int32_T loop_ub ; char_T b_p [ 9 ] ;
char_T b [ 8 ] ; int8_T msubspace_data [ 36 ] ; int8_T tmp [ 6 ] ; boolean_T
b_bool ; static const char_T tmp_p [ 10 ] = { 'd' , 'u' , 'm' , 'm' , 'y' ,
'b' , 'o' , 'd' , 'y' , '9' } ; static const char_T tmp_e [ 5 ] = { 'f' , 'i'
, 'x' , 'e' , 'd' } ; static const char_T tmp_i [ 8 ] = { 'r' , 'e' , 'v' ,
'o' , 'l' , 'u' , 't' , 'e' } ; static const char_T tmp_m [ 9 ] = { 'p' , 'r'
, 'i' , 's' , 'm' , 'a' , 't' , 'i' , 'c' } ; int32_T exitg1 ; b_obj = obj ;
b_kstr = obj -> NameInternal -> size [ 0 ] * obj -> NameInternal -> size [ 1
] ; obj -> NameInternal -> size [ 0 ] = 1 ; obj -> NameInternal -> size [ 1 ]
= 10 ; e3n3mnwk4b ( obj -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ;
b_kstr < 10 ; b_kstr ++ ) { obj -> NameInternal -> data [ b_kstr ] = tmp_p [
b_kstr ] ; } b_kstr = obj -> JointInternal . Type -> size [ 0 ] * obj ->
JointInternal . Type -> size [ 1 ] ; obj -> JointInternal . Type -> size [ 0
] = 1 ; obj -> JointInternal . Type -> size [ 1 ] = 5 ; e3n3mnwk4b ( obj ->
JointInternal . Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ )
{ obj -> JointInternal . Type -> data [ b_kstr ] = tmp_e [ b_kstr ] ; }
m2n1niwoq4 ( & switch_expression , 2 ) ; b_kstr = switch_expression -> size [
0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = obj -> JointInternal . Type -> size [ 1 ] ;
e3n3mnwk4b ( switch_expression , b_kstr ) ; loop_ub = obj -> JointInternal .
Type -> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) {
switch_expression -> data [ b_kstr ] = obj -> JointInternal . Type -> data [
b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { b [ b_kstr ] =
tmp_i [ b_kstr ] ; } b_bool = false ; if ( switch_expression -> size [ 1 ] !=
8 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if (
switch_expression -> data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1 ;
} else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0 ; } else { for ( b_kstr = 0 ;
b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_m [ b_kstr ] ; } if (
switch_expression -> size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1
; } else { b_kstr = - 1 ; } } fagcrr3avn ( & switch_expression ) ; switch (
b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ]
= 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr
++ ) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } obj -> JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 1
] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ; case
1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] = 0 ; tmp [ 4 ]
= 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } obj -> JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 1
] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ;
default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [
b_kstr ] = 0 ; } obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj
-> JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = obj -> JointInternal .
MotionSubspace -> size [ 0 ] * obj -> JointInternal . MotionSubspace -> size
[ 1 ] ; obj -> JointInternal . MotionSubspace -> size [ 0 ] = 6 ; obj ->
JointInternal . MotionSubspace -> size [ 1 ] = 1 ; miyl1rq4uj ( obj ->
JointInternal . MotionSubspace , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 6 ;
b_kstr ++ ) { obj -> JointInternal . MotionSubspace -> data [ b_kstr ] =
msubspace_data [ b_kstr ] ; } obj -> Index = - 1.0 ; obj -> ParentIndex = -
1.0 ; for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) { msubspace_data [ b_kstr
] = 0 ; } for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [
b_kstr + 6 * b_kstr ] = 1 ; } for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) {
obj -> SpatialInertia [ b_kstr ] = msubspace_data [ b_kstr ] ; } p1kacjwiri (
& obj -> CollisionsInternal ) ; obj -> matlabCodegenIsDeleted = false ;
return b_obj ; } static void kff4ccpwjvj ( mjv2tzlyyig * obj , alyuhn31fbf *
iobj_0 ) { e1n154mvbh * switch_expression ; int32_T b_kstr ; int32_T loop_ub
; char_T b_p [ 9 ] ; char_T b [ 8 ] ; int8_T msubspace_data [ 36 ] ; int8_T
tmp [ 6 ] ; boolean_T b_bool ; static const char_T tmp_p [ 10 ] = { 'd' , 'u'
, 'm' , 'm' , 'y' , 'b' , 'o' , 'd' , 'y' , '1' } ; static const char_T tmp_e
[ 5 ] = { 'f' , 'i' , 'x' , 'e' , 'd' } ; static const char_T tmp_i [ 8 ] = {
'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ; static const char_T tmp_m [
9 ] = { 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' , 'i' , 'c' } ; static const
char_T tmp_g [ 10 ] = { 'd' , 'u' , 'm' , 'm' , 'y' , 'b' , 'o' , 'd' , 'y' ,
'2' } ; static const char_T tmp_j [ 10 ] = { 'd' , 'u' , 'm' , 'm' , 'y' ,
'b' , 'o' , 'd' , 'y' , '3' } ; static const char_T tmp_f [ 10 ] = { 'd' ,
'u' , 'm' , 'm' , 'y' , 'b' , 'o' , 'd' , 'y' , '4' } ; int32_T exitg1 ;
b_kstr = iobj_0 [ 0 ] . NameInternal -> size [ 0 ] * iobj_0 [ 0 ] .
NameInternal -> size [ 1 ] ; iobj_0 [ 0 ] . NameInternal -> size [ 0 ] = 1 ;
iobj_0 [ 0 ] . NameInternal -> size [ 1 ] = 10 ; e3n3mnwk4b ( iobj_0 [ 0 ] .
NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 10 ; b_kstr ++ ) {
iobj_0 [ 0 ] . NameInternal -> data [ b_kstr ] = tmp_p [ b_kstr ] ; } b_kstr
= iobj_0 [ 0 ] . JointInternal . Type -> size [ 0 ] * iobj_0 [ 0 ] .
JointInternal . Type -> size [ 1 ] ; iobj_0 [ 0 ] . JointInternal . Type ->
size [ 0 ] = 1 ; iobj_0 [ 0 ] . JointInternal . Type -> size [ 1 ] = 5 ;
e3n3mnwk4b ( iobj_0 [ 0 ] . JointInternal . Type , b_kstr ) ; for ( b_kstr =
0 ; b_kstr < 5 ; b_kstr ++ ) { iobj_0 [ 0 ] . JointInternal . Type -> data [
b_kstr ] = tmp_e [ b_kstr ] ; } m2n1niwoq4 ( & switch_expression , 2 ) ;
b_kstr = switch_expression -> size [ 0 ] * switch_expression -> size [ 1 ] ;
switch_expression -> size [ 0 ] = 1 ; switch_expression -> size [ 1 ] =
iobj_0 [ 0 ] . JointInternal . Type -> size [ 1 ] ; e3n3mnwk4b (
switch_expression , b_kstr ) ; loop_ub = iobj_0 [ 0 ] . JointInternal . Type
-> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) {
switch_expression -> data [ b_kstr ] = iobj_0 [ 0 ] . JointInternal . Type ->
data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { b [ b_kstr
] = tmp_i [ b_kstr ] ; } b_bool = false ; if ( switch_expression -> size [ 1
] != 8 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if
( switch_expression -> data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1
; } else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0 ; } else { for ( b_kstr = 0 ;
b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_m [ b_kstr ] ; } if (
switch_expression -> size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1
; } else { b_kstr = - 1 ; } } switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ;
tmp [ 1 ] = 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0
; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] =
tmp [ b_kstr ] ; } iobj_0 [ 0 ] . JointInternal . JointAxisInternal [ 0 ] =
0.0 ; iobj_0 [ 0 ] . JointInternal . JointAxisInternal [ 1 ] = 0.0 ; iobj_0 [
0 ] . JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ; case 1 : tmp [
0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp
[ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [
b_kstr ] = tmp [ b_kstr ] ; } iobj_0 [ 0 ] . JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; iobj_0 [ 0 ] . JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; iobj_0 [ 0 ] . JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; default : for ( b_kstr = 0 ; b_kstr <
6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = 0 ; } iobj_0 [ 0 ] .
JointInternal . JointAxisInternal [ 0 ] = 0.0 ; iobj_0 [ 0 ] . JointInternal
. JointAxisInternal [ 1 ] = 0.0 ; iobj_0 [ 0 ] . JointInternal .
JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = iobj_0 [ 0 ] .
JointInternal . MotionSubspace -> size [ 0 ] * iobj_0 [ 0 ] . JointInternal .
MotionSubspace -> size [ 1 ] ; iobj_0 [ 0 ] . JointInternal . MotionSubspace
-> size [ 0 ] = 6 ; iobj_0 [ 0 ] . JointInternal . MotionSubspace -> size [ 1
] = 1 ; miyl1rq4uj ( iobj_0 [ 0 ] . JointInternal . MotionSubspace , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { iobj_0 [ 0 ] . JointInternal
. MotionSubspace -> data [ b_kstr ] = msubspace_data [ b_kstr ] ; } iobj_0 [
0 ] . Index = - 1.0 ; iobj_0 [ 0 ] . ParentIndex = - 1.0 ; for ( b_kstr = 0 ;
b_kstr < 36 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = 0 ; } for ( b_kstr =
0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr + 6 * b_kstr ] = 1 ; }
for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) { iobj_0 [ 0 ] . SpatialInertia
[ b_kstr ] = msubspace_data [ b_kstr ] ; } p1kacjwiri ( & iobj_0 [ 0 ] .
CollisionsInternal ) ; iobj_0 [ 0 ] . matlabCodegenIsDeleted = false ; obj ->
Bodies [ 0 ] = & iobj_0 [ 0 ] ; b_kstr = iobj_0 [ 1 ] . NameInternal -> size
[ 0 ] * iobj_0 [ 1 ] . NameInternal -> size [ 1 ] ; iobj_0 [ 1 ] .
NameInternal -> size [ 0 ] = 1 ; iobj_0 [ 1 ] . NameInternal -> size [ 1 ] =
10 ; e3n3mnwk4b ( iobj_0 [ 1 ] . NameInternal , b_kstr ) ; for ( b_kstr = 0 ;
b_kstr < 10 ; b_kstr ++ ) { iobj_0 [ 1 ] . NameInternal -> data [ b_kstr ] =
tmp_g [ b_kstr ] ; } b_kstr = iobj_0 [ 1 ] . JointInternal . Type -> size [ 0
] * iobj_0 [ 1 ] . JointInternal . Type -> size [ 1 ] ; iobj_0 [ 1 ] .
JointInternal . Type -> size [ 0 ] = 1 ; iobj_0 [ 1 ] . JointInternal . Type
-> size [ 1 ] = 5 ; e3n3mnwk4b ( iobj_0 [ 1 ] . JointInternal . Type , b_kstr
) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { iobj_0 [ 1 ] .
JointInternal . Type -> data [ b_kstr ] = tmp_e [ b_kstr ] ; } b_kstr =
switch_expression -> size [ 0 ] * switch_expression -> size [ 1 ] ;
switch_expression -> size [ 0 ] = 1 ; switch_expression -> size [ 1 ] =
iobj_0 [ 1 ] . JointInternal . Type -> size [ 1 ] ; e3n3mnwk4b (
switch_expression , b_kstr ) ; loop_ub = iobj_0 [ 1 ] . JointInternal . Type
-> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) {
switch_expression -> data [ b_kstr ] = iobj_0 [ 1 ] . JointInternal . Type ->
data [ b_kstr ] ; } b_bool = false ; if ( switch_expression -> size [ 1 ] !=
8 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if (
switch_expression -> data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1 ;
} else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0 ; } else { for ( b_kstr = 0 ;
b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_m [ b_kstr ] ; } if (
switch_expression -> size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1
; } else { b_kstr = - 1 ; } } switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ;
tmp [ 1 ] = 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0
; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] =
tmp [ b_kstr ] ; } iobj_0 [ 1 ] . JointInternal . JointAxisInternal [ 0 ] =
0.0 ; iobj_0 [ 1 ] . JointInternal . JointAxisInternal [ 1 ] = 0.0 ; iobj_0 [
1 ] . JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ; case 1 : tmp [
0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp
[ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [
b_kstr ] = tmp [ b_kstr ] ; } iobj_0 [ 1 ] . JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; iobj_0 [ 1 ] . JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; iobj_0 [ 1 ] . JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; default : for ( b_kstr = 0 ; b_kstr <
6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = 0 ; } iobj_0 [ 1 ] .
JointInternal . JointAxisInternal [ 0 ] = 0.0 ; iobj_0 [ 1 ] . JointInternal
. JointAxisInternal [ 1 ] = 0.0 ; iobj_0 [ 1 ] . JointInternal .
JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = iobj_0 [ 1 ] .
JointInternal . MotionSubspace -> size [ 0 ] * iobj_0 [ 1 ] . JointInternal .
MotionSubspace -> size [ 1 ] ; iobj_0 [ 1 ] . JointInternal . MotionSubspace
-> size [ 0 ] = 6 ; iobj_0 [ 1 ] . JointInternal . MotionSubspace -> size [ 1
] = 1 ; miyl1rq4uj ( iobj_0 [ 1 ] . JointInternal . MotionSubspace , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { iobj_0 [ 1 ] . JointInternal
. MotionSubspace -> data [ b_kstr ] = msubspace_data [ b_kstr ] ; } iobj_0 [
1 ] . Index = - 1.0 ; iobj_0 [ 1 ] . ParentIndex = - 1.0 ; for ( b_kstr = 0 ;
b_kstr < 36 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = 0 ; } for ( b_kstr =
0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr + 6 * b_kstr ] = 1 ; }
for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) { iobj_0 [ 1 ] . SpatialInertia
[ b_kstr ] = msubspace_data [ b_kstr ] ; } p1kacjwiri ( & iobj_0 [ 1 ] .
CollisionsInternal ) ; iobj_0 [ 1 ] . matlabCodegenIsDeleted = false ; obj ->
Bodies [ 1 ] = & iobj_0 [ 1 ] ; b_kstr = iobj_0 [ 2 ] . NameInternal -> size
[ 0 ] * iobj_0 [ 2 ] . NameInternal -> size [ 1 ] ; iobj_0 [ 2 ] .
NameInternal -> size [ 0 ] = 1 ; iobj_0 [ 2 ] . NameInternal -> size [ 1 ] =
10 ; e3n3mnwk4b ( iobj_0 [ 2 ] . NameInternal , b_kstr ) ; for ( b_kstr = 0 ;
b_kstr < 10 ; b_kstr ++ ) { iobj_0 [ 2 ] . NameInternal -> data [ b_kstr ] =
tmp_j [ b_kstr ] ; } b_kstr = iobj_0 [ 2 ] . JointInternal . Type -> size [ 0
] * iobj_0 [ 2 ] . JointInternal . Type -> size [ 1 ] ; iobj_0 [ 2 ] .
JointInternal . Type -> size [ 0 ] = 1 ; iobj_0 [ 2 ] . JointInternal . Type
-> size [ 1 ] = 5 ; e3n3mnwk4b ( iobj_0 [ 2 ] . JointInternal . Type , b_kstr
) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { iobj_0 [ 2 ] .
JointInternal . Type -> data [ b_kstr ] = tmp_e [ b_kstr ] ; } b_kstr =
switch_expression -> size [ 0 ] * switch_expression -> size [ 1 ] ;
switch_expression -> size [ 0 ] = 1 ; switch_expression -> size [ 1 ] =
iobj_0 [ 2 ] . JointInternal . Type -> size [ 1 ] ; e3n3mnwk4b (
switch_expression , b_kstr ) ; loop_ub = iobj_0 [ 2 ] . JointInternal . Type
-> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) {
switch_expression -> data [ b_kstr ] = iobj_0 [ 2 ] . JointInternal . Type ->
data [ b_kstr ] ; } b_bool = false ; if ( switch_expression -> size [ 1 ] !=
8 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if (
switch_expression -> data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1 ;
} else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0 ; } else { for ( b_kstr = 0 ;
b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_m [ b_kstr ] ; } if (
switch_expression -> size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1
; } else { b_kstr = - 1 ; } } switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ;
tmp [ 1 ] = 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0
; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] =
tmp [ b_kstr ] ; } iobj_0 [ 2 ] . JointInternal . JointAxisInternal [ 0 ] =
0.0 ; iobj_0 [ 2 ] . JointInternal . JointAxisInternal [ 1 ] = 0.0 ; iobj_0 [
2 ] . JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ; case 1 : tmp [
0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp
[ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [
b_kstr ] = tmp [ b_kstr ] ; } iobj_0 [ 2 ] . JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; iobj_0 [ 2 ] . JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; iobj_0 [ 2 ] . JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; default : for ( b_kstr = 0 ; b_kstr <
6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = 0 ; } iobj_0 [ 2 ] .
JointInternal . JointAxisInternal [ 0 ] = 0.0 ; iobj_0 [ 2 ] . JointInternal
. JointAxisInternal [ 1 ] = 0.0 ; iobj_0 [ 2 ] . JointInternal .
JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = iobj_0 [ 2 ] .
JointInternal . MotionSubspace -> size [ 0 ] * iobj_0 [ 2 ] . JointInternal .
MotionSubspace -> size [ 1 ] ; iobj_0 [ 2 ] . JointInternal . MotionSubspace
-> size [ 0 ] = 6 ; iobj_0 [ 2 ] . JointInternal . MotionSubspace -> size [ 1
] = 1 ; miyl1rq4uj ( iobj_0 [ 2 ] . JointInternal . MotionSubspace , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { iobj_0 [ 2 ] . JointInternal
. MotionSubspace -> data [ b_kstr ] = msubspace_data [ b_kstr ] ; } iobj_0 [
2 ] . Index = - 1.0 ; iobj_0 [ 2 ] . ParentIndex = - 1.0 ; for ( b_kstr = 0 ;
b_kstr < 36 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = 0 ; } for ( b_kstr =
0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr + 6 * b_kstr ] = 1 ; }
for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) { iobj_0 [ 2 ] . SpatialInertia
[ b_kstr ] = msubspace_data [ b_kstr ] ; } p1kacjwiri ( & iobj_0 [ 2 ] .
CollisionsInternal ) ; iobj_0 [ 2 ] . matlabCodegenIsDeleted = false ; obj ->
Bodies [ 2 ] = & iobj_0 [ 2 ] ; b_kstr = iobj_0 [ 3 ] . NameInternal -> size
[ 0 ] * iobj_0 [ 3 ] . NameInternal -> size [ 1 ] ; iobj_0 [ 3 ] .
NameInternal -> size [ 0 ] = 1 ; iobj_0 [ 3 ] . NameInternal -> size [ 1 ] =
10 ; e3n3mnwk4b ( iobj_0 [ 3 ] . NameInternal , b_kstr ) ; for ( b_kstr = 0 ;
b_kstr < 10 ; b_kstr ++ ) { iobj_0 [ 3 ] . NameInternal -> data [ b_kstr ] =
tmp_f [ b_kstr ] ; } b_kstr = iobj_0 [ 3 ] . JointInternal . Type -> size [ 0
] * iobj_0 [ 3 ] . JointInternal . Type -> size [ 1 ] ; iobj_0 [ 3 ] .
JointInternal . Type -> size [ 0 ] = 1 ; iobj_0 [ 3 ] . JointInternal . Type
-> size [ 1 ] = 5 ; e3n3mnwk4b ( iobj_0 [ 3 ] . JointInternal . Type , b_kstr
) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { iobj_0 [ 3 ] .
JointInternal . Type -> data [ b_kstr ] = tmp_e [ b_kstr ] ; } b_kstr =
switch_expression -> size [ 0 ] * switch_expression -> size [ 1 ] ;
switch_expression -> size [ 0 ] = 1 ; switch_expression -> size [ 1 ] =
iobj_0 [ 3 ] . JointInternal . Type -> size [ 1 ] ; e3n3mnwk4b (
switch_expression , b_kstr ) ; loop_ub = iobj_0 [ 3 ] . JointInternal . Type
-> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) {
switch_expression -> data [ b_kstr ] = iobj_0 [ 3 ] . JointInternal . Type ->
data [ b_kstr ] ; } b_bool = false ; if ( switch_expression -> size [ 1 ] !=
8 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if (
switch_expression -> data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1 ;
} else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0 ; } else { for ( b_kstr = 0 ;
b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_m [ b_kstr ] ; } if (
switch_expression -> size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1
; } else { b_kstr = - 1 ; } } fagcrr3avn ( & switch_expression ) ; switch (
b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ]
= 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr
++ ) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } iobj_0 [ 3 ] .
JointInternal . JointAxisInternal [ 0 ] = 0.0 ; iobj_0 [ 3 ] . JointInternal
. JointAxisInternal [ 1 ] = 0.0 ; iobj_0 [ 3 ] . JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; case 1 : tmp [ 0 ] = 0 ; tmp [ 1 ] =
0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 1 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } iobj_0 [ 3 ] . JointInternal . JointAxisInternal [ 0 ] = 0.0 ;
iobj_0 [ 3 ] . JointInternal . JointAxisInternal [ 1 ] = 0.0 ; iobj_0 [ 3 ] .
JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ; default : for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = 0 ; }
iobj_0 [ 3 ] . JointInternal . JointAxisInternal [ 0 ] = 0.0 ; iobj_0 [ 3 ] .
JointInternal . JointAxisInternal [ 1 ] = 0.0 ; iobj_0 [ 3 ] . JointInternal
. JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = iobj_0 [ 3 ] .
JointInternal . MotionSubspace -> size [ 0 ] * iobj_0 [ 3 ] . JointInternal .
MotionSubspace -> size [ 1 ] ; iobj_0 [ 3 ] . JointInternal . MotionSubspace
-> size [ 0 ] = 6 ; iobj_0 [ 3 ] . JointInternal . MotionSubspace -> size [ 1
] = 1 ; miyl1rq4uj ( iobj_0 [ 3 ] . JointInternal . MotionSubspace , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { iobj_0 [ 3 ] . JointInternal
. MotionSubspace -> data [ b_kstr ] = msubspace_data [ b_kstr ] ; } iobj_0 [
3 ] . Index = - 1.0 ; iobj_0 [ 3 ] . ParentIndex = - 1.0 ; for ( b_kstr = 0 ;
b_kstr < 36 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = 0 ; } for ( b_kstr =
0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr + 6 * b_kstr ] = 1 ; }
for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) { iobj_0 [ 3 ] . SpatialInertia
[ b_kstr ] = msubspace_data [ b_kstr ] ; } p1kacjwiri ( & iobj_0 [ 3 ] .
CollisionsInternal ) ; iobj_0 [ 3 ] . matlabCodegenIsDeleted = false ; obj ->
Bodies [ 3 ] = & iobj_0 [ 3 ] ; obj -> Bodies [ 4 ] = f502siufoq ( & iobj_0 [
4 ] ) ; obj -> Bodies [ 5 ] = au3yg4mglc ( & iobj_0 [ 5 ] ) ; obj -> Bodies [
6 ] = ir0wvh3inp ( & iobj_0 [ 6 ] ) ; obj -> Bodies [ 7 ] = neqqfka14z ( &
iobj_0 [ 7 ] ) ; obj -> Bodies [ 8 ] = lafe2rmqud ( & iobj_0 [ 8 ] ) ; }
static alyuhn31fbf * f5m4gscdrl ( alyuhn31fbf * obj ) { alyuhn31fbf * b_obj ;
e1n154mvbh * switch_expression ; int32_T b_kstr ; int32_T loop_ub ; char_T
b_p [ 9 ] ; char_T b [ 8 ] ; int8_T msubspace_data [ 36 ] ; int8_T tmp [ 6 ]
; boolean_T b_bool ; static const char_T tmp_p [ 5 ] = { 'l' , 'i' , 'n' ,
'k' , '3' } ; static const real_T tmp_e [ 36 ] = { 0.0017632141516686539 , -
6.368433412722551E-7 , - 0.00018371649145547766 , 0.0 , - 0.0140866554520649
, 5.22780188213871E-5 , - 6.368433412722551E-7 , 0.0017902634754422204 , -
6.8885291018321461E-6 , 0.0140866554520649 , 0.0 , - 0.00142780695846381 , -
0.00018371649145547766 , - 6.8885291018321461E-6 , 7.50846247383926E-5 , -
5.22780188213871E-5 , 0.00142780695846381 , 0.0 , 0.0 , 0.0140866554520649 ,
- 5.22780188213871E-5 , 0.13850917 , 0.0 , 0.0 , - 0.0140866554520649 , 0.0 ,
0.00142780695846381 , 0.0 , 0.13850917 , 0.0 , 5.22780188213871E-5 , -
0.00142780695846381 , 0.0 , 0.0 , 0.0 , 0.13850917 } ; static const char_T
tmp_i [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ; static
const char_T tmp_m [ 9 ] = { 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' , 'i' ,
'c' } ; static const real_T tmp_g [ 16 ] = { 1.0 , 0.0 , - 0.0 , 0.0 , 0.0 ,
1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.058 , 1.0 } ; static
const real_T tmp_j [ 16 ] = { 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 } ; static const real_T tmp_f [
36 ] = { 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 } ;
int32_T exitg1 ; b_obj = obj ; b_kstr = obj -> NameInternal -> size [ 0 ] *
obj -> NameInternal -> size [ 1 ] ; obj -> NameInternal -> size [ 0 ] = 1 ;
obj -> NameInternal -> size [ 1 ] = 5 ; e3n3mnwk4b ( obj -> NameInternal ,
b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { obj -> NameInternal
-> data [ b_kstr ] = tmp_p [ b_kstr ] ; } obj -> ParentIndex = 2.0 ; for (
b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) { obj -> SpatialInertia [ b_kstr ] =
tmp_e [ b_kstr ] ; } b_kstr = obj -> JointInternal . Type -> size [ 0 ] * obj
-> JointInternal . Type -> size [ 1 ] ; obj -> JointInternal . Type -> size [
0 ] = 1 ; obj -> JointInternal . Type -> size [ 1 ] = 8 ; e3n3mnwk4b ( obj ->
JointInternal . Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ )
{ obj -> JointInternal . Type -> data [ b_kstr ] = tmp_i [ b_kstr ] ; }
m2n1niwoq4 ( & switch_expression , 2 ) ; b_kstr = switch_expression -> size [
0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = obj -> JointInternal . Type -> size [ 1 ] ;
e3n3mnwk4b ( switch_expression , b_kstr ) ; loop_ub = obj -> JointInternal .
Type -> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) {
switch_expression -> data [ b_kstr ] = obj -> JointInternal . Type -> data [
b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { b [ b_kstr ] =
tmp_i [ b_kstr ] ; } b_bool = false ; if ( switch_expression -> size [ 1 ] !=
8 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if (
switch_expression -> data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1 ;
} else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0 ; } else { for ( b_kstr = 0 ;
b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_m [ b_kstr ] ; } if (
switch_expression -> size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1
; } else { b_kstr = - 1 ; } } fagcrr3avn ( & switch_expression ) ; switch (
b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ]
= 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr
++ ) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } obj -> JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 1
] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ; case
1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] = 0 ; tmp [ 4 ]
= 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } obj -> JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 1
] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ;
default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [
b_kstr ] = 0 ; } obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj
-> JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = obj -> JointInternal .
MotionSubspace -> size [ 0 ] * obj -> JointInternal . MotionSubspace -> size
[ 1 ] ; obj -> JointInternal . MotionSubspace -> size [ 0 ] = 6 ; obj ->
JointInternal . MotionSubspace -> size [ 1 ] = 1 ; miyl1rq4uj ( obj ->
JointInternal . MotionSubspace , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 6 ;
b_kstr ++ ) { obj -> JointInternal . MotionSubspace -> data [ b_kstr ] =
msubspace_data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) {
obj -> JointInternal . JointToParentTransform [ b_kstr ] = tmp_g [ b_kstr ] ;
} for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj -> JointInternal .
ChildToJointTransform [ b_kstr ] = tmp_j [ b_kstr ] ; } b_kstr = obj ->
JointInternal . MotionSubspace -> size [ 0 ] * obj -> JointInternal .
MotionSubspace -> size [ 1 ] ; obj -> JointInternal . MotionSubspace -> size
[ 0 ] = 6 ; obj -> JointInternal . MotionSubspace -> size [ 1 ] = 1 ;
miyl1rq4uj ( obj -> JointInternal . MotionSubspace , b_kstr ) ; for ( b_kstr
= 0 ; b_kstr < 6 ; b_kstr ++ ) { obj -> JointInternal . MotionSubspace ->
data [ b_kstr ] = tmp_f [ b_kstr ] ; } obj -> JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 1
] = 1.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 0.0 ; p1kacjwiri (
& obj -> CollisionsInternal ) ; obj -> matlabCodegenIsDeleted = false ;
return b_obj ; } static alyuhn31fbf * fpyihj4bu5 ( alyuhn31fbf * obj ) {
alyuhn31fbf * b_obj ; e1n154mvbh * switch_expression ; int32_T b_kstr ;
int32_T loop_ub ; char_T b_p [ 9 ] ; char_T b [ 8 ] ; int8_T msubspace_data [
36 ] ; int8_T tmp [ 6 ] ; boolean_T b_bool ; static const char_T tmp_p [ 5 ]
= { 'l' , 'i' , 'n' , 'k' , '4' } ; static const real_T tmp_e [ 36 ] = {
3.0680964548160883E-5 , - 5.97440725674762E-6 , - 2.973545034809753E-6 , 0.0
, - 2.9752646170879803E-5 , 5.16776256140592E-5 , - 5.97440725674762E-6 ,
0.001339392874088295 , - 2.3100409454553815E-11 , 2.9752646170879803E-5 , 0.0
, - 0.0120678498884958 , - 2.973545034809753E-6 , - 2.3100409454553815E-11 ,
0.0013486539735492638 , - 5.16776256140592E-5 , 0.0120678498884958 , 0.0 ,
0.0 , 2.9752646170879803E-5 , - 5.16776256140592E-5 , 0.13274562 , 0.0 , 0.0
, - 2.9752646170879803E-5 , 0.0 , 0.0120678498884958 , 0.0 , 0.13274562 , 0.0
, 5.16776256140592E-5 , - 0.0120678498884958 , 0.0 , 0.0 , 0.0 , 0.13274562 }
; static const char_T tmp_i [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't'
, 'e' } ; static const char_T tmp_m [ 9 ] = { 'p' , 'r' , 'i' , 's' , 'm' ,
'a' , 't' , 'i' , 'c' } ; static const real_T tmp_g [ 16 ] = { 1.0 , 0.0 , -
0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.024 , 0.0 ,
0.128 , 1.0 } ; static const real_T tmp_j [ 16 ] = { 1.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 } ;
static const real_T tmp_f [ 36 ] = { 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0
, 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0
, 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0
, 0.0 , 0.0 , 0.0 } ; int32_T exitg1 ; b_obj = obj ; b_kstr = obj ->
NameInternal -> size [ 0 ] * obj -> NameInternal -> size [ 1 ] ; obj ->
NameInternal -> size [ 0 ] = 1 ; obj -> NameInternal -> size [ 1 ] = 5 ;
e3n3mnwk4b ( obj -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ;
b_kstr ++ ) { obj -> NameInternal -> data [ b_kstr ] = tmp_p [ b_kstr ] ; }
obj -> ParentIndex = 3.0 ; for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) { obj
-> SpatialInertia [ b_kstr ] = tmp_e [ b_kstr ] ; } b_kstr = obj ->
JointInternal . Type -> size [ 0 ] * obj -> JointInternal . Type -> size [ 1
] ; obj -> JointInternal . Type -> size [ 0 ] = 1 ; obj -> JointInternal .
Type -> size [ 1 ] = 8 ; e3n3mnwk4b ( obj -> JointInternal . Type , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { obj -> JointInternal . Type
-> data [ b_kstr ] = tmp_i [ b_kstr ] ; } m2n1niwoq4 ( & switch_expression ,
2 ) ; b_kstr = switch_expression -> size [ 0 ] * switch_expression -> size [
1 ] ; switch_expression -> size [ 0 ] = 1 ; switch_expression -> size [ 1 ] =
obj -> JointInternal . Type -> size [ 1 ] ; e3n3mnwk4b ( switch_expression ,
b_kstr ) ; loop_ub = obj -> JointInternal . Type -> size [ 1 ] ; for ( b_kstr
= 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [ b_kstr ] =
obj -> JointInternal . Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr
< 8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_i [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_m [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ]
= 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj ->
JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; case 1 : tmp [ 0 ] = 0 ; tmp [ 1 ] =
0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 1 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj ->
JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; default : for ( b_kstr = 0 ; b_kstr <
6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = 0 ; } obj -> JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 1
] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 0.0 ; break ; }
b_kstr = obj -> JointInternal . MotionSubspace -> size [ 0 ] * obj ->
JointInternal . MotionSubspace -> size [ 1 ] ; obj -> JointInternal .
MotionSubspace -> size [ 0 ] = 6 ; obj -> JointInternal . MotionSubspace ->
size [ 1 ] = 1 ; miyl1rq4uj ( obj -> JointInternal . MotionSubspace , b_kstr
) ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { obj -> JointInternal .
MotionSubspace -> data [ b_kstr ] = msubspace_data [ b_kstr ] ; } for (
b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj -> JointInternal .
JointToParentTransform [ b_kstr ] = tmp_g [ b_kstr ] ; } for ( b_kstr = 0 ;
b_kstr < 16 ; b_kstr ++ ) { obj -> JointInternal . ChildToJointTransform [
b_kstr ] = tmp_j [ b_kstr ] ; } b_kstr = obj -> JointInternal .
MotionSubspace -> size [ 0 ] * obj -> JointInternal . MotionSubspace -> size
[ 1 ] ; obj -> JointInternal . MotionSubspace -> size [ 0 ] = 6 ; obj ->
JointInternal . MotionSubspace -> size [ 1 ] = 1 ; miyl1rq4uj ( obj ->
JointInternal . MotionSubspace , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 6 ;
b_kstr ++ ) { obj -> JointInternal . MotionSubspace -> data [ b_kstr ] =
tmp_f [ b_kstr ] ; } obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ;
obj -> JointInternal . JointAxisInternal [ 1 ] = 1.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 0.0 ; p1kacjwiri ( & obj -> CollisionsInternal ) ;
obj -> matlabCodegenIsDeleted = false ; return b_obj ; } static alyuhn31fbf *
dgvymmhbap ( alyuhn31fbf * obj ) { alyuhn31fbf * b_obj ; e1n154mvbh *
switch_expression ; int32_T b_kstr ; int32_T loop_ub ; char_T b_p [ 9 ] ;
char_T b [ 8 ] ; int8_T msubspace_data [ 36 ] ; int8_T tmp [ 6 ] ; boolean_T
b_bool ; static const char_T tmp_p [ 5 ] = { 'l' , 'i' , 'n' , 'k' , '5' } ;
static const real_T tmp_e [ 36 ] = { 9.2255918030988742E-5 , -
2.3333544264028212E-9 , - 5.747628606408824E-5 , 0.0 , -
0.0012771916071217678 , 5.2782757440640491E-8 , - 2.3333544264028212E-9 ,
0.00036736039366238232 , - 4.705171964849182E-10 , 0.0012771916071217678 ,
0.0 , - 0.00633375509355615 , - 5.747628606408824E-5 , -
4.705171964849182E-10 , 0.00037312211067028395 , - 5.2782757440640491E-8 ,
0.00633375509355615 , 0.0 , 0.0 , 0.0012771916071217678 , -
5.2782757440640491E-8 , 0.14327573 , 0.0 , 0.0 , - 0.0012771916071217678 ,
0.0 , 0.00633375509355615 , 0.0 , 0.14327573 , 0.0 , 5.2782757440640491E-8 ,
- 0.00633375509355615 , 0.0 , 0.0 , 0.0 , 0.14327573 } ; static const char_T
tmp_i [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ; static
const char_T tmp_m [ 9 ] = { 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' , 'i' ,
'c' } ; static const real_T tmp_g [ 16 ] = { 1.0 , 0.0 , - 0.0 , 0.0 , 0.0 ,
1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.124 , 0.0 , 0.0 , 1.0 } ; static
const real_T tmp_j [ 16 ] = { 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 } ; static const real_T tmp_f [
36 ] = { 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 } ;
int32_T exitg1 ; b_obj = obj ; b_kstr = obj -> NameInternal -> size [ 0 ] *
obj -> NameInternal -> size [ 1 ] ; obj -> NameInternal -> size [ 0 ] = 1 ;
obj -> NameInternal -> size [ 1 ] = 5 ; e3n3mnwk4b ( obj -> NameInternal ,
b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { obj -> NameInternal
-> data [ b_kstr ] = tmp_p [ b_kstr ] ; } obj -> ParentIndex = 4.0 ; for (
b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) { obj -> SpatialInertia [ b_kstr ] =
tmp_e [ b_kstr ] ; } b_kstr = obj -> JointInternal . Type -> size [ 0 ] * obj
-> JointInternal . Type -> size [ 1 ] ; obj -> JointInternal . Type -> size [
0 ] = 1 ; obj -> JointInternal . Type -> size [ 1 ] = 8 ; e3n3mnwk4b ( obj ->
JointInternal . Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ )
{ obj -> JointInternal . Type -> data [ b_kstr ] = tmp_i [ b_kstr ] ; }
m2n1niwoq4 ( & switch_expression , 2 ) ; b_kstr = switch_expression -> size [
0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = obj -> JointInternal . Type -> size [ 1 ] ;
e3n3mnwk4b ( switch_expression , b_kstr ) ; loop_ub = obj -> JointInternal .
Type -> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) {
switch_expression -> data [ b_kstr ] = obj -> JointInternal . Type -> data [
b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { b [ b_kstr ] =
tmp_i [ b_kstr ] ; } b_bool = false ; if ( switch_expression -> size [ 1 ] !=
8 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if (
switch_expression -> data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1 ;
} else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0 ; } else { for ( b_kstr = 0 ;
b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_m [ b_kstr ] ; } if (
switch_expression -> size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1
; } else { b_kstr = - 1 ; } } fagcrr3avn ( & switch_expression ) ; switch (
b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ]
= 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr
++ ) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } obj -> JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 1
] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ; case
1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] = 0 ; tmp [ 4 ]
= 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } obj -> JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 1
] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ;
default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [
b_kstr ] = 0 ; } obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj
-> JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = obj -> JointInternal .
MotionSubspace -> size [ 0 ] * obj -> JointInternal . MotionSubspace -> size
[ 1 ] ; obj -> JointInternal . MotionSubspace -> size [ 0 ] = 6 ; obj ->
JointInternal . MotionSubspace -> size [ 1 ] = 1 ; miyl1rq4uj ( obj ->
JointInternal . MotionSubspace , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 6 ;
b_kstr ++ ) { obj -> JointInternal . MotionSubspace -> data [ b_kstr ] =
msubspace_data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) {
obj -> JointInternal . JointToParentTransform [ b_kstr ] = tmp_g [ b_kstr ] ;
} for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj -> JointInternal .
ChildToJointTransform [ b_kstr ] = tmp_j [ b_kstr ] ; } b_kstr = obj ->
JointInternal . MotionSubspace -> size [ 0 ] * obj -> JointInternal .
MotionSubspace -> size [ 1 ] ; obj -> JointInternal . MotionSubspace -> size
[ 0 ] = 6 ; obj -> JointInternal . MotionSubspace -> size [ 1 ] = 1 ;
miyl1rq4uj ( obj -> JointInternal . MotionSubspace , b_kstr ) ; for ( b_kstr
= 0 ; b_kstr < 6 ; b_kstr ++ ) { obj -> JointInternal . MotionSubspace ->
data [ b_kstr ] = tmp_f [ b_kstr ] ; } obj -> JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 1
] = 1.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 0.0 ; p1kacjwiri (
& obj -> CollisionsInternal ) ; obj -> matlabCodegenIsDeleted = false ;
return b_obj ; } static alyuhn31fbf * dqkobvx1cl ( alyuhn31fbf * obj ) {
alyuhn31fbf * b_obj ; e1n154mvbh * switch_expression ; int32_T b_kstr ;
int32_T loop_ub ; char_T b_p [ 9 ] ; char_T b [ 8 ] ; int8_T msubspace_data [
36 ] ; int8_T tmp [ 6 ] ; boolean_T b_bool ; static const char_T tmp_p [ 17 ]
= { 'e' , 'n' , 'd' , '_' , 'e' , 'f' , 'f' , 'e' , 'c' , 't' , 'o' , 'r' ,
'_' , 'l' , 'i' , 'n' , 'k' } ; static const real_T tmp_e [ 36 ] = { 1.0E-6 ,
0.0 , 0.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 1.0E-6 , 0.0 , 0.0 , 0.0 , - 0.0 , 0.0
, 0.0 , 1.0E-6 , - 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , - 0.0 , 0.001 , 0.0 , 0.0 ,
- 0.0 , 0.0 , 0.0 , 0.0 , 0.001 , 0.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 0.0 , 0.001
} ; static const char_T tmp_i [ 5 ] = { 'f' , 'i' , 'x' , 'e' , 'd' } ;
static const char_T tmp_m [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't' ,
'e' } ; static const char_T tmp_g [ 9 ] = { 'p' , 'r' , 'i' , 's' , 'm' , 'a'
, 't' , 'i' , 'c' } ; static const real_T tmp_j [ 16 ] = { 1.0 , 0.0 , - 0.0
, 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.13 , 0.0 , 0.0 ,
1.0 } ; static const real_T tmp_f [ 16 ] = { 1.0 , 0.0 , 0.0 , 0.0 , 0.0 ,
1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 } ; int32_T
exitg1 ; b_obj = obj ; b_kstr = obj -> NameInternal -> size [ 0 ] * obj ->
NameInternal -> size [ 1 ] ; obj -> NameInternal -> size [ 0 ] = 1 ; obj ->
NameInternal -> size [ 1 ] = 17 ; e3n3mnwk4b ( obj -> NameInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 17 ; b_kstr ++ ) { obj -> NameInternal -> data
[ b_kstr ] = tmp_p [ b_kstr ] ; } obj -> ParentIndex = 5.0 ; for ( b_kstr = 0
; b_kstr < 36 ; b_kstr ++ ) { obj -> SpatialInertia [ b_kstr ] = tmp_e [
b_kstr ] ; } b_kstr = obj -> JointInternal . Type -> size [ 0 ] * obj ->
JointInternal . Type -> size [ 1 ] ; obj -> JointInternal . Type -> size [ 0
] = 1 ; obj -> JointInternal . Type -> size [ 1 ] = 5 ; e3n3mnwk4b ( obj ->
JointInternal . Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ )
{ obj -> JointInternal . Type -> data [ b_kstr ] = tmp_i [ b_kstr ] ; }
m2n1niwoq4 ( & switch_expression , 2 ) ; b_kstr = switch_expression -> size [
0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = obj -> JointInternal . Type -> size [ 1 ] ;
e3n3mnwk4b ( switch_expression , b_kstr ) ; loop_ub = obj -> JointInternal .
Type -> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) {
switch_expression -> data [ b_kstr ] = obj -> JointInternal . Type -> data [
b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { b [ b_kstr ] =
tmp_m [ b_kstr ] ; } b_bool = false ; if ( switch_expression -> size [ 1 ] !=
8 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if (
switch_expression -> data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1 ;
} else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0 ; } else { for ( b_kstr = 0 ;
b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_g [ b_kstr ] ; } if (
switch_expression -> size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1
; } else { b_kstr = - 1 ; } } fagcrr3avn ( & switch_expression ) ; switch (
b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ]
= 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr
++ ) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } obj -> JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 1
] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ; case
1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] = 0 ; tmp [ 4 ]
= 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } obj -> JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 1
] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ;
default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [
b_kstr ] = 0 ; } obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj
-> JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = obj -> JointInternal .
MotionSubspace -> size [ 0 ] * obj -> JointInternal . MotionSubspace -> size
[ 1 ] ; obj -> JointInternal . MotionSubspace -> size [ 0 ] = 6 ; obj ->
JointInternal . MotionSubspace -> size [ 1 ] = 1 ; miyl1rq4uj ( obj ->
JointInternal . MotionSubspace , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 6 ;
b_kstr ++ ) { obj -> JointInternal . MotionSubspace -> data [ b_kstr ] =
msubspace_data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) {
obj -> JointInternal . JointToParentTransform [ b_kstr ] = tmp_j [ b_kstr ] ;
} for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj -> JointInternal .
ChildToJointTransform [ b_kstr ] = tmp_f [ b_kstr ] ; } b_kstr = obj ->
JointInternal . MotionSubspace -> size [ 0 ] * obj -> JointInternal .
MotionSubspace -> size [ 1 ] ; obj -> JointInternal . MotionSubspace -> size
[ 0 ] = 6 ; obj -> JointInternal . MotionSubspace -> size [ 1 ] = 1 ;
miyl1rq4uj ( obj -> JointInternal . MotionSubspace , b_kstr ) ; for ( b_kstr
= 0 ; b_kstr < 6 ; b_kstr ++ ) { obj -> JointInternal . MotionSubspace ->
data [ b_kstr ] = 0.0 ; } obj -> JointInternal . JointAxisInternal [ 0 ] =
0.0 ; obj -> JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj ->
JointInternal . JointAxisInternal [ 2 ] = 0.0 ; p1kacjwiri ( & obj ->
CollisionsInternal ) ; obj -> matlabCodegenIsDeleted = false ; return b_obj ;
} static alyuhn31fbf * ax1vg3c3wq ( alyuhn31fbf * obj ) { alyuhn31fbf * b_obj
; e1n154mvbh * switch_expression ; int32_T b_kstr ; int32_T loop_ub ; char_T
b_p [ 9 ] ; char_T b [ 8 ] ; int8_T msubspace_data [ 36 ] ; int8_T tmp [ 6 ]
; boolean_T b_bool ; static const char_T tmp_p [ 12 ] = { 'g' , 'r' , 'i' ,
'p' , 'p' , 'e' , 'r' , '_' , 'l' , 'i' , 'n' , 'k' } ; static const real_T
tmp_e [ 36 ] = { 0.001 , 0.0 , 0.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 0.001 , 0.0 ,
0.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 0.001 , - 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , - 0.0
, 0.017 , 0.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 0.0 , 0.017 , 0.0 , 0.0 , - 0.0 ,
0.0 , 0.0 , 0.0 , 0.017 } ; static const char_T tmp_i [ 9 ] = { 'p' , 'r' ,
'i' , 's' , 'm' , 'a' , 't' , 'i' , 'c' } ; static const char_T tmp_m [ 8 ] =
{ 'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ; static const real_T tmp_g
[ 16 ] = { 1.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0
, 0.0 , 0.0817 , 0.019 , 0.0 , 1.0 } ; static const real_T tmp_j [ 16 ] = {
1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 1.0 } ; static const real_T tmp_f [ 36 ] = { 0.0 , 0.0 , 0.0 ,
0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 } ; int32_T exitg1 ; b_obj = obj ;
b_kstr = obj -> NameInternal -> size [ 0 ] * obj -> NameInternal -> size [ 1
] ; obj -> NameInternal -> size [ 0 ] = 1 ; obj -> NameInternal -> size [ 1 ]
= 12 ; e3n3mnwk4b ( obj -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ;
b_kstr < 12 ; b_kstr ++ ) { obj -> NameInternal -> data [ b_kstr ] = tmp_p [
b_kstr ] ; } obj -> ParentIndex = 5.0 ; for ( b_kstr = 0 ; b_kstr < 36 ;
b_kstr ++ ) { obj -> SpatialInertia [ b_kstr ] = tmp_e [ b_kstr ] ; } b_kstr
= obj -> JointInternal . Type -> size [ 0 ] * obj -> JointInternal . Type ->
size [ 1 ] ; obj -> JointInternal . Type -> size [ 0 ] = 1 ; obj ->
JointInternal . Type -> size [ 1 ] = 9 ; e3n3mnwk4b ( obj -> JointInternal .
Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { obj ->
JointInternal . Type -> data [ b_kstr ] = tmp_i [ b_kstr ] ; } m2n1niwoq4 ( &
switch_expression , 2 ) ; b_kstr = switch_expression -> size [ 0 ] *
switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = obj -> JointInternal . Type -> size [ 1 ] ;
e3n3mnwk4b ( switch_expression , b_kstr ) ; loop_ub = obj -> JointInternal .
Type -> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) {
switch_expression -> data [ b_kstr ] = obj -> JointInternal . Type -> data [
b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { b [ b_kstr ] =
tmp_m [ b_kstr ] ; } b_bool = false ; if ( switch_expression -> size [ 1 ] !=
8 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if (
switch_expression -> data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1 ;
} else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0 ; } else { for ( b_kstr = 0 ;
b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_i [ b_kstr ] ; } if (
switch_expression -> size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1
; } else { b_kstr = - 1 ; } } fagcrr3avn ( & switch_expression ) ; switch (
b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ]
= 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr
++ ) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } obj -> JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 1
] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ; case
1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] = 0 ; tmp [ 4 ]
= 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } obj -> JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 1
] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ;
default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [
b_kstr ] = 0 ; } obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj
-> JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = obj -> JointInternal .
MotionSubspace -> size [ 0 ] * obj -> JointInternal . MotionSubspace -> size
[ 1 ] ; obj -> JointInternal . MotionSubspace -> size [ 0 ] = 6 ; obj ->
JointInternal . MotionSubspace -> size [ 1 ] = 1 ; miyl1rq4uj ( obj ->
JointInternal . MotionSubspace , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 6 ;
b_kstr ++ ) { obj -> JointInternal . MotionSubspace -> data [ b_kstr ] =
msubspace_data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) {
obj -> JointInternal . JointToParentTransform [ b_kstr ] = tmp_g [ b_kstr ] ;
} for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj -> JointInternal .
ChildToJointTransform [ b_kstr ] = tmp_j [ b_kstr ] ; } b_kstr = obj ->
JointInternal . MotionSubspace -> size [ 0 ] * obj -> JointInternal .
MotionSubspace -> size [ 1 ] ; obj -> JointInternal . MotionSubspace -> size
[ 0 ] = 6 ; obj -> JointInternal . MotionSubspace -> size [ 1 ] = 1 ;
miyl1rq4uj ( obj -> JointInternal . MotionSubspace , b_kstr ) ; for ( b_kstr
= 0 ; b_kstr < 6 ; b_kstr ++ ) { obj -> JointInternal . MotionSubspace ->
data [ b_kstr ] = tmp_f [ b_kstr ] ; } obj -> JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 1
] = 1.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 0.0 ; p1kacjwiri (
& obj -> CollisionsInternal ) ; obj -> matlabCodegenIsDeleted = false ;
return b_obj ; } static alyuhn31fbf * lful00nwr0 ( alyuhn31fbf * obj ) {
alyuhn31fbf * b_obj ; e1n154mvbh * switch_expression ; int32_T b_kstr ;
int32_T loop_ub ; char_T b_p [ 9 ] ; char_T b [ 8 ] ; int8_T msubspace_data [
36 ] ; int8_T tmp [ 6 ] ; boolean_T b_bool ; static const char_T tmp_p [ 16 ]
= { 'g' , 'r' , 'i' , 'p' , 'p' , 'e' , 'r' , '_' , 'l' , 'i' , 'n' , 'k' ,
'_' , 's' , 'u' , 'b' } ; static const real_T tmp_e [ 36 ] = { 0.001 , 0.0 ,
0.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 0.001 , 0.0 , 0.0 , 0.0 , - 0.0 , 0.0 , 0.0 ,
0.001 , - 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , - 0.0 , 0.017 , 0.0 , 0.0 , - 0.0 ,
0.0 , 0.0 , 0.0 , 0.017 , 0.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 0.0 , 0.017 } ;
static const char_T tmp_i [ 9 ] = { 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' ,
'i' , 'c' } ; static const char_T tmp_m [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l'
, 'u' , 't' , 'e' } ; static const real_T tmp_g [ 16 ] = { 1.0 , 0.0 , - 0.0
, 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0817 , - 0.019 ,
0.0 , 1.0 } ; static const real_T tmp_j [ 16 ] = { 1.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 } ;
static const real_T tmp_f [ 36 ] = { 0.0 , 0.0 , 0.0 , 0.0 , - 1.0 , 0.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 0.0 , 0.0 } ; int32_T exitg1 ; b_obj = obj ; b_kstr = obj ->
NameInternal -> size [ 0 ] * obj -> NameInternal -> size [ 1 ] ; obj ->
NameInternal -> size [ 0 ] = 1 ; obj -> NameInternal -> size [ 1 ] = 16 ;
e3n3mnwk4b ( obj -> NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 16
; b_kstr ++ ) { obj -> NameInternal -> data [ b_kstr ] = tmp_p [ b_kstr ] ; }
obj -> ParentIndex = 5.0 ; for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) { obj
-> SpatialInertia [ b_kstr ] = tmp_e [ b_kstr ] ; } b_kstr = obj ->
JointInternal . Type -> size [ 0 ] * obj -> JointInternal . Type -> size [ 1
] ; obj -> JointInternal . Type -> size [ 0 ] = 1 ; obj -> JointInternal .
Type -> size [ 1 ] = 9 ; e3n3mnwk4b ( obj -> JointInternal . Type , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { obj -> JointInternal . Type
-> data [ b_kstr ] = tmp_i [ b_kstr ] ; } m2n1niwoq4 ( & switch_expression ,
2 ) ; b_kstr = switch_expression -> size [ 0 ] * switch_expression -> size [
1 ] ; switch_expression -> size [ 0 ] = 1 ; switch_expression -> size [ 1 ] =
obj -> JointInternal . Type -> size [ 1 ] ; e3n3mnwk4b ( switch_expression ,
b_kstr ) ; loop_ub = obj -> JointInternal . Type -> size [ 1 ] ; for ( b_kstr
= 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [ b_kstr ] =
obj -> JointInternal . Type -> data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr
< 8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_m [ b_kstr ] ; } b_bool = false ; if (
switch_expression -> size [ 1 ] != 8 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0
; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] =
tmp_i [ b_kstr ] ; } if ( switch_expression -> size [ 1 ] != 9 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } } fagcrr3avn ( &
switch_expression ) ; switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ]
= 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj ->
JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; case 1 : tmp [ 0 ] = 0 ; tmp [ 1 ] =
0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 1 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj ->
JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; default : for ( b_kstr = 0 ; b_kstr <
6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = 0 ; } obj -> JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 1
] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 0.0 ; break ; }
b_kstr = obj -> JointInternal . MotionSubspace -> size [ 0 ] * obj ->
JointInternal . MotionSubspace -> size [ 1 ] ; obj -> JointInternal .
MotionSubspace -> size [ 0 ] = 6 ; obj -> JointInternal . MotionSubspace ->
size [ 1 ] = 1 ; miyl1rq4uj ( obj -> JointInternal . MotionSubspace , b_kstr
) ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { obj -> JointInternal .
MotionSubspace -> data [ b_kstr ] = msubspace_data [ b_kstr ] ; } for (
b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj -> JointInternal .
JointToParentTransform [ b_kstr ] = tmp_g [ b_kstr ] ; } for ( b_kstr = 0 ;
b_kstr < 16 ; b_kstr ++ ) { obj -> JointInternal . ChildToJointTransform [
b_kstr ] = tmp_j [ b_kstr ] ; } b_kstr = obj -> JointInternal .
MotionSubspace -> size [ 0 ] * obj -> JointInternal . MotionSubspace -> size
[ 1 ] ; obj -> JointInternal . MotionSubspace -> size [ 0 ] = 6 ; obj ->
JointInternal . MotionSubspace -> size [ 1 ] = 1 ; miyl1rq4uj ( obj ->
JointInternal . MotionSubspace , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 6 ;
b_kstr ++ ) { obj -> JointInternal . MotionSubspace -> data [ b_kstr ] =
tmp_f [ b_kstr ] ; } obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ;
obj -> JointInternal . JointAxisInternal [ 1 ] = - 1.0 ; obj -> JointInternal
. JointAxisInternal [ 2 ] = 0.0 ; p1kacjwiri ( & obj -> CollisionsInternal )
; obj -> matlabCodegenIsDeleted = false ; return b_obj ; } static alyuhn31fbf
* ph5qebjzgw ( alyuhn31fbf * obj ) { alyuhn31fbf * b_obj ; e1n154mvbh *
switch_expression ; int32_T b_kstr ; int32_T loop_ub ; char_T b_p [ 9 ] ;
char_T b [ 8 ] ; int8_T msubspace_data [ 36 ] ; int8_T tmp [ 6 ] ; boolean_T
b_bool ; static const char_T tmp_p [ 12 ] = { 'e' , 'n' , 'd' , '_' , 'e' ,
'f' , 'f' , 'e' , 'c' , 't' , 'o' , 'r' } ; static const real_T tmp_e [ 36 ]
= { 0.0 , 0.0 , 0.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , - 0.0
, 0.0 , 0.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 0.0 ,
- 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 0.0 , 0.0 } ;
static const char_T tmp_i [ 5 ] = { 'f' , 'i' , 'x' , 'e' , 'd' } ; static
const char_T tmp_m [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' }
; static const char_T tmp_g [ 9 ] = { 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't'
, 'i' , 'c' } ; static const real_T tmp_j [ 16 ] = { 1.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.12 , 0.0 , 0.0 , 1.0 } ;
static const real_T tmp_f [ 16 ] = { 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0
, 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 } ; int32_T exitg1 ;
b_obj = obj ; b_kstr = obj -> NameInternal -> size [ 0 ] * obj ->
NameInternal -> size [ 1 ] ; obj -> NameInternal -> size [ 0 ] = 1 ; obj ->
NameInternal -> size [ 1 ] = 12 ; e3n3mnwk4b ( obj -> NameInternal , b_kstr )
; for ( b_kstr = 0 ; b_kstr < 12 ; b_kstr ++ ) { obj -> NameInternal -> data
[ b_kstr ] = tmp_p [ b_kstr ] ; } obj -> ParentIndex = 5.0 ; for ( b_kstr = 0
; b_kstr < 36 ; b_kstr ++ ) { obj -> SpatialInertia [ b_kstr ] = tmp_e [
b_kstr ] ; } b_kstr = obj -> JointInternal . Type -> size [ 0 ] * obj ->
JointInternal . Type -> size [ 1 ] ; obj -> JointInternal . Type -> size [ 0
] = 1 ; obj -> JointInternal . Type -> size [ 1 ] = 5 ; e3n3mnwk4b ( obj ->
JointInternal . Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ )
{ obj -> JointInternal . Type -> data [ b_kstr ] = tmp_i [ b_kstr ] ; }
m2n1niwoq4 ( & switch_expression , 2 ) ; b_kstr = switch_expression -> size [
0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0 ] = 1 ;
switch_expression -> size [ 1 ] = obj -> JointInternal . Type -> size [ 1 ] ;
e3n3mnwk4b ( switch_expression , b_kstr ) ; loop_ub = obj -> JointInternal .
Type -> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) {
switch_expression -> data [ b_kstr ] = obj -> JointInternal . Type -> data [
b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { b [ b_kstr ] =
tmp_m [ b_kstr ] ; } b_bool = false ; if ( switch_expression -> size [ 1 ] !=
8 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if (
switch_expression -> data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1 ;
} else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0 ; } else { for ( b_kstr = 0 ;
b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_g [ b_kstr ] ; } if (
switch_expression -> size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1
; } else { b_kstr = - 1 ; } } fagcrr3avn ( & switch_expression ) ; switch (
b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ]
= 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr
++ ) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } obj -> JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 1
] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ; case
1 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] = 0 ; tmp [ 4 ]
= 0 ; tmp [ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) {
msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } obj -> JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 1
] = 0.0 ; obj -> JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ;
default : for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [
b_kstr ] = 0 ; } obj -> JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj
-> JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> JointInternal .
JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = obj -> JointInternal .
MotionSubspace -> size [ 0 ] * obj -> JointInternal . MotionSubspace -> size
[ 1 ] ; obj -> JointInternal . MotionSubspace -> size [ 0 ] = 6 ; obj ->
JointInternal . MotionSubspace -> size [ 1 ] = 1 ; miyl1rq4uj ( obj ->
JointInternal . MotionSubspace , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 6 ;
b_kstr ++ ) { obj -> JointInternal . MotionSubspace -> data [ b_kstr ] =
msubspace_data [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) {
obj -> JointInternal . JointToParentTransform [ b_kstr ] = tmp_j [ b_kstr ] ;
} for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj -> JointInternal .
ChildToJointTransform [ b_kstr ] = tmp_f [ b_kstr ] ; } b_kstr = obj ->
JointInternal . MotionSubspace -> size [ 0 ] * obj -> JointInternal .
MotionSubspace -> size [ 1 ] ; obj -> JointInternal . MotionSubspace -> size
[ 0 ] = 6 ; obj -> JointInternal . MotionSubspace -> size [ 1 ] = 1 ;
miyl1rq4uj ( obj -> JointInternal . MotionSubspace , b_kstr ) ; for ( b_kstr
= 0 ; b_kstr < 6 ; b_kstr ++ ) { obj -> JointInternal . MotionSubspace ->
data [ b_kstr ] = 0.0 ; } obj -> JointInternal . JointAxisInternal [ 0 ] =
0.0 ; obj -> JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj ->
JointInternal . JointAxisInternal [ 2 ] = 0.0 ; p1kacjwiri ( & obj ->
CollisionsInternal ) ; obj -> matlabCodegenIsDeleted = false ; return b_obj ;
} static void ep2455cw5msd ( jd4q4eqa1g * obj ) { e1n154mvbh *
switch_expression ; mjv2tzlyyig * obj_p ; int32_T b_kstr ; int32_T loop_ub ;
char_T b_p [ 9 ] ; char_T b [ 8 ] ; int8_T msubspace_data [ 36 ] ; int8_T tmp
[ 6 ] ; boolean_T b_bool ; static const char_T tmp_p [ 5 ] = { 'l' , 'i' ,
'n' , 'k' , '1' } ; static const real_T tmp_e [ 36 ] = {
1.2506407081630646E-5 , 0.0 , - 1.755774678296497E-7 , 0.0 ,
9.63401131614482E-6 , 0.0 , 0.0 , 2.1907079879448666E-5 , 0.0 , -
9.63401131614482E-6 , 0.0 , - 2.4429201311861481E-5 , - 1.755774678296497E-7
, 0.0 , 1.9274903797818021E-5 , - 0.0 , 2.4429201311861481E-5 , 0.0 , 0.0 , -
9.63401131614482E-6 , - 0.0 , 0.079119962 , 0.0 , 0.0 , 9.63401131614482E-6 ,
0.0 , 2.4429201311861481E-5 , 0.0 , 0.079119962 , 0.0 , 0.0 , -
2.4429201311861481E-5 , 0.0 , 0.0 , 0.0 , 0.079119962 } ; static const char_T
tmp_i [ 5 ] = { 'f' , 'i' , 'x' , 'e' , 'd' } ; static const char_T tmp_m [ 8
] = { 'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ; static const char_T
tmp_g [ 9 ] = { 'p' , 'r' , 'i' , 's' , 'm' , 'a' , 't' , 'i' , 'c' } ;
static const real_T tmp_j [ 16 ] = { 1.0 , 0.0 , - 0.0 , 0.0 , 0.0 , 1.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 } ; static const
real_T tmp_f [ 16 ] = { 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 } ; static const char_T tmp_c [ 5 ] =
{ 'l' , 'i' , 'n' , 'k' , '2' } ; static const real_T tmp_k [ 36 ] = {
0.00025597667209157054 , 2.2027827360445393E-11 , 1.0251971350492796E-6 , 0.0
, - 0.004667731499421 , 5.3182680022675086E-5 , 2.2027827360445393E-11 ,
0.00025410280331852224 , - 2.4941021265155473E-6 , 0.004667731499421 , 0.0 ,
2.9703975819561897E-5 , 1.0251971350492796E-6 , - 2.4941021265155473E-6 ,
1.8888027986020151E-5 , - 5.3182680022675086E-5 , - 2.9703975819561897E-5 ,
0.0 , 0.0 , 0.004667731499421 , - 5.3182680022675086E-5 , 0.098406837 , 0.0 ,
0.0 , - 0.004667731499421 , 0.0 , - 2.9703975819561897E-5 , 0.0 , 0.098406837
, 0.0 , 5.3182680022675086E-5 , 2.9703975819561897E-5 , 0.0 , 0.0 , 0.0 ,
0.098406837 } ; static const real_T tmp_b [ 16 ] = { 1.0 , 0.0 , - 0.0 , 0.0
, 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.012 , 0.0 , 0.017 , 1.0 }
; static const real_T tmp_n [ 36 ] = { 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 0.0 , 0.0 } ; static const int8_T tmp_l [ 18 ] = { 0 , 1 , 2 , 3
, 4 , 0 , 5 , 6 , 0 , - 1 , 1 , 2 , 3 , 4 , - 1 , 5 , 6 , - 1 } ; static
const char_T tmp_d [ 5 ] = { 'w' , 'o' , 'r' , 'l' , 'd' } ; int32_T exitg1 ;
obj -> isInitialized = 1 ; obj_p = & obj -> TreeInternal ; obj ->
TreeInternal . NumBodies = 9.0 ; kff4ccpwjvj ( & obj -> TreeInternal , & obj
-> TreeInternal . _pobj0 [ 0 ] ) ; b_kstr = obj_p -> _pobj0 [ 9 ] .
NameInternal -> size [ 0 ] * obj_p -> _pobj0 [ 9 ] . NameInternal -> size [ 1
] ; obj_p -> _pobj0 [ 9 ] . NameInternal -> size [ 0 ] = 1 ; obj_p -> _pobj0
[ 9 ] . NameInternal -> size [ 1 ] = 5 ; e3n3mnwk4b ( obj_p -> _pobj0 [ 9 ] .
NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { obj_p
-> _pobj0 [ 9 ] . NameInternal -> data [ b_kstr ] = tmp_p [ b_kstr ] ; }
obj_p -> _pobj0 [ 9 ] . ParentIndex = 0.0 ; for ( b_kstr = 0 ; b_kstr < 36 ;
b_kstr ++ ) { obj_p -> _pobj0 [ 9 ] . SpatialInertia [ b_kstr ] = tmp_e [
b_kstr ] ; } b_kstr = obj -> TreeInternal . _pobj0 [ 9 ] . JointInternal .
Type -> size [ 0 ] * obj -> TreeInternal . _pobj0 [ 9 ] . JointInternal .
Type -> size [ 1 ] ; obj -> TreeInternal . _pobj0 [ 9 ] . JointInternal .
Type -> size [ 0 ] = 1 ; obj -> TreeInternal . _pobj0 [ 9 ] . JointInternal .
Type -> size [ 1 ] = 5 ; e3n3mnwk4b ( obj -> TreeInternal . _pobj0 [ 9 ] .
JointInternal . Type , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ )
{ obj -> TreeInternal . _pobj0 [ 9 ] . JointInternal . Type -> data [ b_kstr
] = tmp_i [ b_kstr ] ; } m2n1niwoq4 ( & switch_expression , 2 ) ; b_kstr =
switch_expression -> size [ 0 ] * switch_expression -> size [ 1 ] ;
switch_expression -> size [ 0 ] = 1 ; switch_expression -> size [ 1 ] = obj
-> TreeInternal . _pobj0 [ 9 ] . JointInternal . Type -> size [ 1 ] ;
e3n3mnwk4b ( switch_expression , b_kstr ) ; loop_ub = obj -> TreeInternal .
_pobj0 [ 9 ] . JointInternal . Type -> size [ 1 ] ; for ( b_kstr = 0 ; b_kstr
< loop_ub ; b_kstr ++ ) { switch_expression -> data [ b_kstr ] = obj ->
TreeInternal . _pobj0 [ 9 ] . JointInternal . Type -> data [ b_kstr ] ; } for
( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { b [ b_kstr ] = tmp_m [ b_kstr ] ; }
b_bool = false ; if ( switch_expression -> size [ 1 ] != 8 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 0 ; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++
) { b_p [ b_kstr ] = tmp_g [ b_kstr ] ; } if ( switch_expression -> size [ 1
] != 9 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if
( switch_expression -> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 =
1 ; } else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } }
switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 1 ;
tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for ( b_kstr = 0 ; b_kstr < 6
; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } obj ->
TreeInternal . _pobj0 [ 9 ] . JointInternal . JointAxisInternal [ 0 ] = 0.0 ;
obj -> TreeInternal . _pobj0 [ 9 ] . JointInternal . JointAxisInternal [ 1 ]
= 0.0 ; obj -> TreeInternal . _pobj0 [ 9 ] . JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; case 1 : tmp [ 0 ] = 0 ; tmp [ 1 ] =
0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 1 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } obj -> TreeInternal . _pobj0 [ 9 ] . JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> TreeInternal . _pobj0 [ 9 ] .
JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> TreeInternal . _pobj0
[ 9 ] . JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ; default : for
( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = 0 ; }
obj -> TreeInternal . _pobj0 [ 9 ] . JointInternal . JointAxisInternal [ 0 ]
= 0.0 ; obj -> TreeInternal . _pobj0 [ 9 ] . JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; obj -> TreeInternal . _pobj0 [ 9 ] .
JointInternal . JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = obj ->
TreeInternal . _pobj0 [ 9 ] . JointInternal . MotionSubspace -> size [ 0 ] *
obj -> TreeInternal . _pobj0 [ 9 ] . JointInternal . MotionSubspace -> size [
1 ] ; obj -> TreeInternal . _pobj0 [ 9 ] . JointInternal . MotionSubspace ->
size [ 0 ] = 6 ; obj -> TreeInternal . _pobj0 [ 9 ] . JointInternal .
MotionSubspace -> size [ 1 ] = 1 ; miyl1rq4uj ( obj -> TreeInternal . _pobj0
[ 9 ] . JointInternal . MotionSubspace , b_kstr ) ; for ( b_kstr = 0 ; b_kstr
< 6 ; b_kstr ++ ) { obj -> TreeInternal . _pobj0 [ 9 ] . JointInternal .
MotionSubspace -> data [ b_kstr ] = msubspace_data [ b_kstr ] ; } for (
b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj_p -> _pobj0 [ 9 ] .
JointInternal . JointToParentTransform [ b_kstr ] = tmp_j [ b_kstr ] ; } for
( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj_p -> _pobj0 [ 9 ] .
JointInternal . ChildToJointTransform [ b_kstr ] = tmp_f [ b_kstr ] ; }
b_kstr = obj_p -> _pobj0 [ 9 ] . JointInternal . MotionSubspace -> size [ 0 ]
* obj_p -> _pobj0 [ 9 ] . JointInternal . MotionSubspace -> size [ 1 ] ;
obj_p -> _pobj0 [ 9 ] . JointInternal . MotionSubspace -> size [ 0 ] = 6 ;
obj_p -> _pobj0 [ 9 ] . JointInternal . MotionSubspace -> size [ 1 ] = 1 ;
miyl1rq4uj ( obj_p -> _pobj0 [ 9 ] . JointInternal . MotionSubspace , b_kstr
) ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { obj_p -> _pobj0 [ 9 ] .
JointInternal . MotionSubspace -> data [ b_kstr ] = 0.0 ; } obj_p -> _pobj0 [
9 ] . JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj_p -> _pobj0 [ 9 ] .
JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj_p -> _pobj0 [ 9 ] .
JointInternal . JointAxisInternal [ 2 ] = 0.0 ; p1kacjwiri ( & obj_p ->
_pobj0 [ 9 ] . CollisionsInternal ) ; obj_p -> _pobj0 [ 9 ] .
matlabCodegenIsDeleted = false ; obj -> TreeInternal . Bodies [ 0 ] = & obj_p
-> _pobj0 [ 9 ] ; obj -> TreeInternal . Bodies [ 0 ] -> Index = 1.0 ; b_kstr
= obj_p -> _pobj0 [ 10 ] . NameInternal -> size [ 0 ] * obj_p -> _pobj0 [ 10
] . NameInternal -> size [ 1 ] ; obj_p -> _pobj0 [ 10 ] . NameInternal ->
size [ 0 ] = 1 ; obj_p -> _pobj0 [ 10 ] . NameInternal -> size [ 1 ] = 5 ;
e3n3mnwk4b ( obj_p -> _pobj0 [ 10 ] . NameInternal , b_kstr ) ; for ( b_kstr
= 0 ; b_kstr < 5 ; b_kstr ++ ) { obj_p -> _pobj0 [ 10 ] . NameInternal ->
data [ b_kstr ] = tmp_c [ b_kstr ] ; } obj_p -> _pobj0 [ 10 ] . ParentIndex =
1.0 ; for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) { obj_p -> _pobj0 [ 10 ] .
SpatialInertia [ b_kstr ] = tmp_k [ b_kstr ] ; } b_kstr = obj -> TreeInternal
. _pobj0 [ 10 ] . JointInternal . Type -> size [ 0 ] * obj -> TreeInternal .
_pobj0 [ 10 ] . JointInternal . Type -> size [ 1 ] ; obj -> TreeInternal .
_pobj0 [ 10 ] . JointInternal . Type -> size [ 0 ] = 1 ; obj -> TreeInternal
. _pobj0 [ 10 ] . JointInternal . Type -> size [ 1 ] = 8 ; e3n3mnwk4b ( obj
-> TreeInternal . _pobj0 [ 10 ] . JointInternal . Type , b_kstr ) ; for (
b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { obj -> TreeInternal . _pobj0 [ 10 ] .
JointInternal . Type -> data [ b_kstr ] = tmp_m [ b_kstr ] ; } b_kstr =
switch_expression -> size [ 0 ] * switch_expression -> size [ 1 ] ;
switch_expression -> size [ 0 ] = 1 ; switch_expression -> size [ 1 ] = obj
-> TreeInternal . _pobj0 [ 10 ] . JointInternal . Type -> size [ 1 ] ;
e3n3mnwk4b ( switch_expression , b_kstr ) ; loop_ub = obj -> TreeInternal .
_pobj0 [ 10 ] . JointInternal . Type -> size [ 1 ] ; for ( b_kstr = 0 ;
b_kstr < loop_ub ; b_kstr ++ ) { switch_expression -> data [ b_kstr ] = obj
-> TreeInternal . _pobj0 [ 10 ] . JointInternal . Type -> data [ b_kstr ] ; }
b_bool = false ; if ( switch_expression -> size [ 1 ] != 8 ) { } else {
b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if ( switch_expression
-> data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_kstr = 0 ; } else { for ( b_kstr = 0 ; b_kstr < 9 ; b_kstr ++
) { b_p [ b_kstr ] = tmp_g [ b_kstr ] ; } if ( switch_expression -> size [ 1
] != 9 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 9 ) { if
( switch_expression -> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) { exitg1 =
1 ; } else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1 ; } else { b_kstr = - 1 ; } }
switch ( b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 1 ;
tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for ( b_kstr = 0 ; b_kstr < 6
; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } obj ->
TreeInternal . _pobj0 [ 10 ] . JointInternal . JointAxisInternal [ 0 ] = 0.0
; obj -> TreeInternal . _pobj0 [ 10 ] . JointInternal . JointAxisInternal [ 1
] = 0.0 ; obj -> TreeInternal . _pobj0 [ 10 ] . JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; case 1 : tmp [ 0 ] = 0 ; tmp [ 1 ] =
0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 1 ; for (
b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = tmp [
b_kstr ] ; } obj -> TreeInternal . _pobj0 [ 10 ] . JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> TreeInternal . _pobj0 [ 10 ] .
JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> TreeInternal . _pobj0
[ 10 ] . JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ; default :
for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = 0 ;
} obj -> TreeInternal . _pobj0 [ 10 ] . JointInternal . JointAxisInternal [ 0
] = 0.0 ; obj -> TreeInternal . _pobj0 [ 10 ] . JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; obj -> TreeInternal . _pobj0 [ 10 ] .
JointInternal . JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = obj ->
TreeInternal . _pobj0 [ 10 ] . JointInternal . MotionSubspace -> size [ 0 ] *
obj -> TreeInternal . _pobj0 [ 10 ] . JointInternal . MotionSubspace -> size
[ 1 ] ; obj -> TreeInternal . _pobj0 [ 10 ] . JointInternal . MotionSubspace
-> size [ 0 ] = 6 ; obj -> TreeInternal . _pobj0 [ 10 ] . JointInternal .
MotionSubspace -> size [ 1 ] = 1 ; miyl1rq4uj ( obj -> TreeInternal . _pobj0
[ 10 ] . JointInternal . MotionSubspace , b_kstr ) ; for ( b_kstr = 0 ;
b_kstr < 6 ; b_kstr ++ ) { obj -> TreeInternal . _pobj0 [ 10 ] .
JointInternal . MotionSubspace -> data [ b_kstr ] = msubspace_data [ b_kstr ]
; } for ( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj_p -> _pobj0 [ 10 ] .
JointInternal . JointToParentTransform [ b_kstr ] = tmp_b [ b_kstr ] ; } for
( b_kstr = 0 ; b_kstr < 16 ; b_kstr ++ ) { obj_p -> _pobj0 [ 10 ] .
JointInternal . ChildToJointTransform [ b_kstr ] = tmp_f [ b_kstr ] ; }
b_kstr = obj_p -> _pobj0 [ 10 ] . JointInternal . MotionSubspace -> size [ 0
] * obj_p -> _pobj0 [ 10 ] . JointInternal . MotionSubspace -> size [ 1 ] ;
obj_p -> _pobj0 [ 10 ] . JointInternal . MotionSubspace -> size [ 0 ] = 6 ;
obj_p -> _pobj0 [ 10 ] . JointInternal . MotionSubspace -> size [ 1 ] = 1 ;
miyl1rq4uj ( obj_p -> _pobj0 [ 10 ] . JointInternal . MotionSubspace , b_kstr
) ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { obj_p -> _pobj0 [ 10 ] .
JointInternal . MotionSubspace -> data [ b_kstr ] = tmp_n [ b_kstr ] ; }
obj_p -> _pobj0 [ 10 ] . JointInternal . JointAxisInternal [ 0 ] = 0.0 ;
obj_p -> _pobj0 [ 10 ] . JointInternal . JointAxisInternal [ 1 ] = 0.0 ;
obj_p -> _pobj0 [ 10 ] . JointInternal . JointAxisInternal [ 2 ] = 1.0 ;
p1kacjwiri ( & obj_p -> _pobj0 [ 10 ] . CollisionsInternal ) ; obj_p ->
_pobj0 [ 10 ] . matlabCodegenIsDeleted = false ; obj -> TreeInternal . Bodies
[ 1 ] = & obj_p -> _pobj0 [ 10 ] ; obj -> TreeInternal . Bodies [ 1 ] ->
Index = 2.0 ; obj -> TreeInternal . Bodies [ 2 ] = f5m4gscdrl ( & obj ->
TreeInternal . _pobj0 [ 11 ] ) ; obj -> TreeInternal . Bodies [ 2 ] -> Index
= 3.0 ; obj -> TreeInternal . Bodies [ 3 ] = fpyihj4bu5 ( & obj ->
TreeInternal . _pobj0 [ 12 ] ) ; obj -> TreeInternal . Bodies [ 3 ] -> Index
= 4.0 ; obj -> TreeInternal . Bodies [ 4 ] = dgvymmhbap ( & obj ->
TreeInternal . _pobj0 [ 13 ] ) ; obj -> TreeInternal . Bodies [ 4 ] -> Index
= 5.0 ; obj -> TreeInternal . Bodies [ 5 ] = dqkobvx1cl ( & obj ->
TreeInternal . _pobj0 [ 14 ] ) ; obj -> TreeInternal . Bodies [ 5 ] -> Index
= 6.0 ; obj -> TreeInternal . Bodies [ 6 ] = ax1vg3c3wq ( & obj ->
TreeInternal . _pobj0 [ 15 ] ) ; obj -> TreeInternal . Bodies [ 6 ] -> Index
= 7.0 ; obj -> TreeInternal . Bodies [ 7 ] = lful00nwr0 ( & obj ->
TreeInternal . _pobj0 [ 16 ] ) ; obj -> TreeInternal . Bodies [ 7 ] -> Index
= 8.0 ; obj -> TreeInternal . Bodies [ 8 ] = ph5qebjzgw ( & obj ->
TreeInternal . _pobj0 [ 17 ] ) ; obj -> TreeInternal . Bodies [ 8 ] -> Index
= 9.0 ; obj -> TreeInternal . Gravity [ 0 ] = 0.0 ; obj -> TreeInternal .
Gravity [ 1 ] = 0.0 ; obj -> TreeInternal . Gravity [ 2 ] = - 9.80665 ; for (
b_kstr = 0 ; b_kstr < 18 ; b_kstr ++ ) { obj -> TreeInternal . PositionDoFMap
[ b_kstr ] = tmp_l [ b_kstr ] ; } for ( b_kstr = 0 ; b_kstr < 18 ; b_kstr ++
) { obj -> TreeInternal . VelocityDoFMap [ b_kstr ] = tmp_l [ b_kstr ] ; }
b_kstr = obj_p -> Base . NameInternal -> size [ 0 ] * obj_p -> Base .
NameInternal -> size [ 1 ] ; obj_p -> Base . NameInternal -> size [ 0 ] = 1 ;
obj_p -> Base . NameInternal -> size [ 1 ] = 5 ; e3n3mnwk4b ( obj_p -> Base .
NameInternal , b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 5 ; b_kstr ++ ) { obj_p
-> Base . NameInternal -> data [ b_kstr ] = tmp_d [ b_kstr ] ; } obj_p ->
Base . ParentIndex = - 1.0 ; for ( b_kstr = 0 ; b_kstr < 36 ; b_kstr ++ ) {
obj_p -> Base . SpatialInertia [ b_kstr ] = 0.0 ; } b_kstr = obj ->
TreeInternal . Base . JointInternal . Type -> size [ 0 ] * obj ->
TreeInternal . Base . JointInternal . Type -> size [ 1 ] ; obj ->
TreeInternal . Base . JointInternal . Type -> size [ 0 ] = 1 ; obj ->
TreeInternal . Base . JointInternal . Type -> size [ 1 ] = 5 ; e3n3mnwk4b (
obj -> TreeInternal . Base . JointInternal . Type , b_kstr ) ; for ( b_kstr =
0 ; b_kstr < 5 ; b_kstr ++ ) { obj -> TreeInternal . Base . JointInternal .
Type -> data [ b_kstr ] = tmp_i [ b_kstr ] ; } b_kstr = switch_expression ->
size [ 0 ] * switch_expression -> size [ 1 ] ; switch_expression -> size [ 0
] = 1 ; switch_expression -> size [ 1 ] = obj -> TreeInternal . Base .
JointInternal . Type -> size [ 1 ] ; e3n3mnwk4b ( switch_expression , b_kstr
) ; loop_ub = obj -> TreeInternal . Base . JointInternal . Type -> size [ 1 ]
; for ( b_kstr = 0 ; b_kstr < loop_ub ; b_kstr ++ ) { switch_expression ->
data [ b_kstr ] = obj -> TreeInternal . Base . JointInternal . Type -> data [
b_kstr ] ; } b_bool = false ; if ( switch_expression -> size [ 1 ] != 8 ) { }
else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if (
switch_expression -> data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1 ;
} else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 0 ; } else { for ( b_kstr = 0 ;
b_kstr < 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_g [ b_kstr ] ; } if (
switch_expression -> size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 =
0 ; if ( b_kstr - 1 < 9 ) { if ( switch_expression -> data [ b_kstr - 1 ] !=
b_p [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_kstr = 1
; } else { b_kstr = - 1 ; } } fagcrr3avn ( & switch_expression ) ; switch (
b_kstr ) { case 0 : tmp [ 0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 1 ; tmp [ 3 ]
= 0 ; tmp [ 4 ] = 0 ; tmp [ 5 ] = 0 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr
++ ) { msubspace_data [ b_kstr ] = tmp [ b_kstr ] ; } obj -> TreeInternal .
Base . JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj -> TreeInternal .
Base . JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> TreeInternal .
Base . JointInternal . JointAxisInternal [ 2 ] = 1.0 ; break ; case 1 : tmp [
0 ] = 0 ; tmp [ 1 ] = 0 ; tmp [ 2 ] = 0 ; tmp [ 3 ] = 0 ; tmp [ 4 ] = 0 ; tmp
[ 5 ] = 1 ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { msubspace_data [
b_kstr ] = tmp [ b_kstr ] ; } obj -> TreeInternal . Base . JointInternal .
JointAxisInternal [ 0 ] = 0.0 ; obj -> TreeInternal . Base . JointInternal .
JointAxisInternal [ 1 ] = 0.0 ; obj -> TreeInternal . Base . JointInternal .
JointAxisInternal [ 2 ] = 1.0 ; break ; default : for ( b_kstr = 0 ; b_kstr <
6 ; b_kstr ++ ) { msubspace_data [ b_kstr ] = 0 ; } obj -> TreeInternal .
Base . JointInternal . JointAxisInternal [ 0 ] = 0.0 ; obj -> TreeInternal .
Base . JointInternal . JointAxisInternal [ 1 ] = 0.0 ; obj -> TreeInternal .
Base . JointInternal . JointAxisInternal [ 2 ] = 0.0 ; break ; } b_kstr = obj
-> TreeInternal . Base . JointInternal . MotionSubspace -> size [ 0 ] * obj
-> TreeInternal . Base . JointInternal . MotionSubspace -> size [ 1 ] ; obj
-> TreeInternal . Base . JointInternal . MotionSubspace -> size [ 0 ] = 6 ;
obj -> TreeInternal . Base . JointInternal . MotionSubspace -> size [ 1 ] = 1
; miyl1rq4uj ( obj -> TreeInternal . Base . JointInternal . MotionSubspace ,
b_kstr ) ; for ( b_kstr = 0 ; b_kstr < 6 ; b_kstr ++ ) { obj -> TreeInternal
. Base . JointInternal . MotionSubspace -> data [ b_kstr ] = msubspace_data [
b_kstr ] ; } p1kacjwiri ( & obj_p -> Base . CollisionsInternal ) ; obj_p ->
Base . matlabCodegenIsDeleted = false ; obj -> TreeInternal . Base . Index =
0.0 ; obj -> TreeInternal . matlabCodegenIsDeleted = false ; } static void
fv2pu0tbva ( lbiyaleu3n * * pEmxArray , int32_T numDimensions ) { lbiyaleu3n
* emxArray ; int32_T i ; * pEmxArray = ( lbiyaleu3n * ) malloc ( sizeof (
lbiyaleu3n ) ) ; emxArray = * pEmxArray ; emxArray -> data = ( int8_T * )
NULL ; emxArray -> numDimensions = numDimensions ; emxArray -> size = (
int32_T * ) malloc ( sizeof ( int32_T ) * ( uint32_T ) numDimensions ) ;
emxArray -> allocatedSize = 0 ; emxArray -> canFreeData = true ; for ( i = 0
; i < numDimensions ; i ++ ) { emxArray -> size [ i ] = 0 ; } } static void
n10lmcng5l ( lbiyaleu3n * emxArray , int32_T oldNumel ) { int32_T i ; int32_T
newNumel ; void * newData ; if ( oldNumel < 0 ) { oldNumel = 0 ; } newNumel =
1 ; for ( i = 0 ; i < emxArray -> numDimensions ; i ++ ) { newNumel *=
emxArray -> size [ i ] ; } if ( newNumel > emxArray -> allocatedSize ) { i =
emxArray -> allocatedSize ; if ( i < 16 ) { i = 16 ; } while ( i < newNumel )
{ if ( i > 1073741823 ) { i = MAX_int32_T ; } else { i <<= 1 ; } } newData =
calloc ( ( uint32_T ) i , sizeof ( int8_T ) ) ; if ( emxArray -> data != NULL
) { memcpy ( newData , emxArray -> data , sizeof ( int8_T ) * ( uint32_T )
oldNumel ) ; if ( emxArray -> canFreeData ) { free ( emxArray -> data ) ; } }
emxArray -> data = ( int8_T * ) newData ; emxArray -> allocatedSize = i ;
emxArray -> canFreeData = true ; } } static void bqvhlmvfpz ( lbiyaleu3n * *
pEmxArray ) { if ( * pEmxArray != ( lbiyaleu3n * ) NULL ) { if ( ( ( *
pEmxArray ) -> data != ( int8_T * ) NULL ) && ( * pEmxArray ) -> canFreeData
) { free ( ( * pEmxArray ) -> data ) ; } free ( ( * pEmxArray ) -> size ) ;
free ( * pEmxArray ) ; * pEmxArray = ( lbiyaleu3n * ) NULL ; } } static void
lij5c2bvo0 ( m0caqfktx0 * obj , klgljlxbmf * limits ) { alyuhn31fbfz * body ;
e1n154mvbh * a ; ot4uovzkt0w * obj_p ; real_T k ; real_T pnum ; int32_T
b_kstr ; int32_T c ; int32_T i ; int32_T limits_p ; int32_T loop_ub ; char_T
b [ 5 ] ; boolean_T b_bool ; static const char_T tmp [ 5 ] = { 'f' , 'i' ,
'x' , 'e' , 'd' } ; int32_T exitg1 ; int32_T i_p ; i_p = limits -> size [ 0 ]
* limits -> size [ 1 ] ; limits -> size [ 0 ] = ( int32_T ) obj ->
PositionNumber ; limits -> size [ 1 ] = 2 ; miyl1rq4uj ( limits , i_p ) ;
loop_ub = ( int32_T ) obj -> PositionNumber << 1 ; if ( loop_ub - 1 >= 0 ) {
memset ( & limits -> data [ 0 ] , 0 , ( uint32_T ) loop_ub * sizeof ( real_T
) ) ; } k = 1.0 ; pnum = obj -> NumBodies ; c = ( int32_T ) pnum - 1 ; if ( (
int32_T ) pnum - 1 >= 0 ) { for ( i_p = 0 ; i_p < 5 ; i_p ++ ) { b [ i_p ] =
tmp [ i_p ] ; } } m2n1niwoq4 ( & a , 2 ) ; for ( limits_p = 0 ; limits_p <= c
; limits_p ++ ) { body = obj -> Bodies [ limits_p ] ; i_p = a -> size [ 0 ] *
a -> size [ 1 ] ; a -> size [ 0 ] = 1 ; a -> size [ 1 ] = body ->
JointInternal -> Type -> size [ 1 ] ; e3n3mnwk4b ( a , i_p ) ; loop_ub = body
-> JointInternal -> Type -> size [ 1 ] ; for ( i_p = 0 ; i_p < loop_ub ; i_p
++ ) { a -> data [ i_p ] = body -> JointInternal -> Type -> data [ i_p ] ; }
b_bool = false ; if ( a -> size [ 1 ] != 5 ) { } else { b_kstr = 1 ; do {
exitg1 = 0 ; if ( b_kstr - 1 < 5 ) { if ( a -> data [ b_kstr - 1 ] != b [
b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool = true ;
exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( ! b_bool ) { pnum = body ->
JointInternal -> PositionNumber ; if ( k > ( k + pnum ) - 1.0 ) { b_kstr = 0
; } else { b_kstr = ( int32_T ) k - 1 ; } obj_p = body -> JointInternal ;
loop_ub = obj_p -> PositionLimitsInternal -> size [ 0 ] ; for ( i_p = 0 ; i_p
< 2 ; i_p ++ ) { for ( i = 0 ; i < loop_ub ; i ++ ) { limits -> data [ (
b_kstr + i ) + limits -> size [ 0 ] * i_p ] = obj_p -> PositionLimitsInternal
-> data [ obj_p -> PositionLimitsInternal -> size [ 0 ] * i_p + i ] ; } } k
+= pnum ; } } fagcrr3avn ( & a ) ; } static void lh3fakbjip ( boolean_T in1 [
6 ] , const real_T in2 [ 6 ] , const klgljlxbmf * in3 ) { int32_T i ; int32_T
stride_0_0 ; stride_0_0 = ( in3 -> size [ 0 ] != 1 ) ; for ( i = 0 ; i < 6 ;
i ++ ) { in1 [ i ] = ( in2 [ i ] <= in3 -> data [ i * stride_0_0 + in3 ->
size [ 0 ] ] + 4.4408920985006262E-16 ) ; } } static void offa2br1khoegki (
boolean_T in1 [ 6 ] , const real_T in2 [ 6 ] , const klgljlxbmf * in3 ) {
int32_T i ; int32_T stride_0_0 ; stride_0_0 = ( in3 -> size [ 0 ] != 1 ) ;
for ( i = 0 ; i < 6 ; i ++ ) { in1 [ i ] = ( in2 [ i ] >= in3 -> data [ i *
stride_0_0 ] - 4.4408920985006262E-16 ) ; } } static void mfmaegtoxt ( const
boolean_T x [ 6 ] , int32_T i_data [ ] , int32_T * i_size ) { int32_T b_ii ;
int32_T idx ; boolean_T exitg1 ; idx = 0 ; b_ii = 1 ; exitg1 = false ; while
( ( ! exitg1 ) && ( b_ii - 1 < 6 ) ) { if ( x [ b_ii - 1 ] ) { idx ++ ;
i_data [ idx - 1 ] = b_ii ; if ( idx >= 6 ) { exitg1 = true ; } else { b_ii
++ ; } } else { b_ii ++ ; } } if ( idx < 1 ) { idx = 0 ; } * i_size = idx ; }
static void ot2wucsrdj ( real_T * tstart_tv_sec , real_T * tstart_tv_nsec ) {
coderTimespec b_timespec ; if ( ! rtDW . gcwd0zaucf ) { rtDW . gcwd0zaucf =
true ; coderInitTimeFunctions ( & rtDW . ccbb0ktavq ) ; }
coderTimeClockGettimeMonotonic ( & b_timespec , rtDW . ccbb0ktavq ) ; *
tstart_tv_sec = b_timespec . tv_sec ; * tstart_tv_nsec = b_timespec . tv_nsec
; } static void e3pxqiajks ( m0caqfktx0 * obj , alyuhn31fbfz * body ,
klgljlxbmf * indices ) { real_T i ; int32_T loop_ub ; loop_ub = indices ->
size [ 0 ] * indices -> size [ 1 ] ; indices -> size [ 0 ] = 1 ; indices ->
size [ 1 ] = ( int32_T ) ( obj -> NumBodies + 1.0 ) ; miyl1rq4uj ( indices ,
loop_ub ) ; loop_ub = ( int32_T ) ( obj -> NumBodies + 1.0 ) ; if ( loop_ub -
1 >= 0 ) { memset ( & indices -> data [ 0 ] , 0 , ( uint32_T ) loop_ub *
sizeof ( real_T ) ) ; } i = 2.0 ; indices -> data [ 0 ] = body -> Index ;
while ( body -> ParentIndex > 0.0 ) { body = obj -> Bodies [ ( int32_T ) body
-> ParentIndex - 1 ] ; indices -> data [ ( int32_T ) i - 1 ] = body -> Index
; i ++ ; } if ( body -> Index > 0.0 ) { indices -> data [ ( int32_T ) i - 1 ]
= body -> ParentIndex ; i ++ ; } loop_ub = indices -> size [ 0 ] * indices ->
size [ 1 ] ; indices -> size [ 0 ] = 1 ; indices -> size [ 1 ] = ( int32_T )
( i - 1.0 ) ; miyl1rq4uj ( indices , loop_ub ) ; } static void k4jhhplgmj (
m0caqfktx0 * obj , alyuhn31fbfz * body1 , alyuhn31fbfz * body2 , klgljlxbmf *
indices ) { klgljlxbmf * ancestorIndices1 ; klgljlxbmf * ancestorIndices2 ;
int32_T b_i ; int32_T d ; int32_T g ; int32_T h ; int32_T i ; int32_T
minPathLength ; boolean_T exitg1 ; calykgtvmm ( & ancestorIndices1 , 2 ) ;
e3pxqiajks ( obj , body1 , ancestorIndices1 ) ; calykgtvmm ( &
ancestorIndices2 , 2 ) ; e3pxqiajks ( obj , body2 , ancestorIndices2 ) ;
minPathLength = ( int32_T ) muDoubleScalarMin ( ancestorIndices1 -> size [ 1
] , ancestorIndices2 -> size [ 1 ] ) ; b_i = 0 ; exitg1 = false ; while ( ( !
exitg1 ) && ( b_i <= minPathLength - 2 ) ) { if ( ancestorIndices1 -> data [
( ancestorIndices1 -> size [ 1 ] - b_i ) - 2 ] != ancestorIndices2 -> data [
( ancestorIndices2 -> size [ 1 ] - b_i ) - 2 ] ) { minPathLength = b_i + 1 ;
exitg1 = true ; } else { b_i ++ ; } } d = ancestorIndices1 -> size [ 1 ] -
minPathLength ; if ( d < 1 ) { b_i = 0 ; } else { b_i = d ; } d =
ancestorIndices2 -> size [ 1 ] - minPathLength ; if ( d < 1 ) { d = 1 ; h = 1
; g = 0 ; } else { h = - 1 ; g = 1 ; } i = indices -> size [ 0 ] * indices ->
size [ 1 ] ; indices -> size [ 0 ] = 1 ; indices -> size [ 1 ] = ( div_s32 (
g - d , h ) + b_i ) + 2 ; miyl1rq4uj ( indices , i ) ; if ( b_i - 1 >= 0 ) {
memcpy ( & indices -> data [ 0 ] , & ancestorIndices1 -> data [ 0 ] , (
uint32_T ) b_i * sizeof ( real_T ) ) ; } indices -> data [ b_i ] =
ancestorIndices1 -> data [ ancestorIndices1 -> size [ 1 ] - minPathLength ] ;
bdhoypqe5x ( & ancestorIndices1 ) ; minPathLength = div_s32 ( g - d , h ) ;
for ( i = 0 ; i <= minPathLength ; i ++ ) { indices -> data [ ( i + b_i ) + 1
] = ancestorIndices2 -> data [ ( h * i + d ) - 1 ] ; } bdhoypqe5x ( &
ancestorIndices2 ) ; } static void cnixr4pd3ac ( const ot4uovzkt0w * obj ,
real_T ax [ 3 ] ) { int32_T b_kstr ; char_T b_p [ 9 ] ; char_T b [ 8 ] ;
boolean_T b_bool ; static const char_T tmp [ 8 ] = { 'r' , 'e' , 'v' , 'o' ,
'l' , 'u' , 't' , 'e' } ; static const char_T tmp_p [ 9 ] = { 'p' , 'r' , 'i'
, 's' , 'm' , 'a' , 't' , 'i' , 'c' } ; int32_T exitg1 ; boolean_T guard1 =
false ; for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { b [ b_kstr ] = tmp [
b_kstr ] ; } b_bool = false ; if ( obj -> Type -> size [ 1 ] != 8 ) { } else
{ b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if ( obj -> Type ->
data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ;
} } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } guard1
= false ; if ( b_bool ) { guard1 = true ; } else { for ( b_kstr = 0 ; b_kstr
< 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_p [ b_kstr ] ; } if ( obj -> Type ->
size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 <
9 ) { if ( obj -> Type -> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) {
exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } }
while ( exitg1 == 0 ) ; } if ( b_bool ) { guard1 = true ; } else { ax [ 0 ] =
( rtNaN ) ; ax [ 1 ] = ( rtNaN ) ; ax [ 2 ] = ( rtNaN ) ; } } if ( guard1 ) {
ax [ 0 ] = obj -> JointAxisInternal [ 0 ] ; ax [ 1 ] = obj ->
JointAxisInternal [ 1 ] ; ax [ 2 ] = obj -> JointAxisInternal [ 2 ] ; } }
static void lpd2uo4ns5 ( real_T varargin_1 , real_T varargin_2 , real_T
varargin_3 , real_T varargin_4 , real_T varargin_5 , real_T varargin_6 ,
real_T varargin_7 , real_T varargin_8 , real_T varargin_9 , real_T y [ 9 ] )
{ y [ 0 ] = varargin_1 ; y [ 1 ] = varargin_2 ; y [ 2 ] = varargin_3 ; y [ 3
] = varargin_4 ; y [ 4 ] = varargin_5 ; y [ 5 ] = varargin_6 ; y [ 6 ] =
varargin_7 ; y [ 7 ] = varargin_8 ; y [ 8 ] = varargin_9 ; } static void
l5ks21uytp ( const real_T A [ 36 ] , const klgljlxbmf * B_p , klgljlxbmf * C
) { real_T s ; int32_T b_i ; int32_T b_j ; int32_T b_k ; int32_T boffset ;
int32_T coffset ; int32_T n ; n = B_p -> size [ 1 ] - 1 ; b_j = C -> size [ 0
] * C -> size [ 1 ] ; C -> size [ 0 ] = 6 ; C -> size [ 1 ] = B_p -> size [ 1
] ; miyl1rq4uj ( C , b_j ) ; for ( b_j = 0 ; b_j <= n ; b_j ++ ) { coffset =
b_j * 6 - 1 ; boffset = b_j * 6 - 1 ; for ( b_i = 0 ; b_i < 6 ; b_i ++ ) { s
= 0.0 ; for ( b_k = 0 ; b_k < 6 ; b_k ++ ) { s += A [ b_k * 6 + b_i ] * B_p
-> data [ ( boffset + b_k ) + 1 ] ; } C -> data [ ( coffset + b_i ) + 1 ] = s
; } } } static void p3z13glynv ( m0caqfktx0 * obj , const real_T qv [ 6 ] ,
const e1n154mvbh * body1Name , real_T T_data [ ] , int32_T T_size [ 2 ] ,
klgljlxbmf * Jac ) { __m128d tmp_p ; alyuhn31fbfz * body1 ; alyuhn31fbfz *
body2 ; e1n154mvbh * body2Name ; klgljlxbmf * b ; klgljlxbmf *
kinematicPathIndices ; klgljlxbmf * tmp ; ot4uovzkt0w * joint ; real_T Tj_e [
36 ] ; real_T T1 [ 16 ] ; real_T T1j [ 16 ] ; real_T Tc2p [ 16 ] ; real_T Tj
[ 16 ] ; real_T Tj_p [ 16 ] ; real_T b_e [ 16 ] ; real_T R [ 9 ] ; real_T
tempR [ 9 ] ; real_T result_data [ 4 ] ; real_T v [ 3 ] ; real_T axang_idx_2
; real_T bid1 ; real_T bid2 ; real_T qidx_idx_1 ; int32_T Jac_p ; int32_T c ;
int32_T f ; int32_T g ; int32_T i ; int32_T loop_ub ; char_T b_i [ 8 ] ;
char_T b_p [ 5 ] ; boolean_T b_bool ; boolean_T nextBodyIsParent ; static
const char_T tmp_e [ 5 ] = { 'f' , 'i' , 'x' , 'e' , 'd' } ; static const
char_T tmp_i [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ;
klgljlxbmf * Jac_e ; int32_T exitg1 ; m2n1niwoq4 ( & body2Name , 2 ) ; i =
body2Name -> size [ 0 ] * body2Name -> size [ 1 ] ; body2Name -> size [ 0 ] =
1 ; body2Name -> size [ 1 ] = obj -> Base . NameInternal -> size [ 1 ] ;
e3n3mnwk4b ( body2Name , i ) ; loop_ub = obj -> Base . NameInternal -> size [
1 ] ; for ( i = 0 ; i < loop_ub ; i ++ ) { body2Name -> data [ i ] = obj ->
Base . NameInternal -> data [ i ] ; } bid1 = jjyjgxfdua ( obj , body1Name ) ;
bid2 = jjyjgxfdua ( obj , body2Name ) ; if ( bid1 == 0.0 ) { body1 = & obj ->
Base ; } else { body1 = obj -> Bodies [ ( int32_T ) bid1 - 1 ] ; } if ( bid2
== 0.0 ) { body2 = & obj -> Base ; } else { body2 = obj -> Bodies [ ( int32_T
) bid2 - 1 ] ; } calykgtvmm ( & kinematicPathIndices , 2 ) ; k4jhhplgmj ( obj
, body1 , body2 , kinematicPathIndices ) ; memset ( & T1 [ 0 ] , 0 , sizeof (
real_T ) << 4U ) ; T1 [ 0 ] = 1.0 ; T1 [ 5 ] = 1.0 ; T1 [ 10 ] = 1.0 ; T1 [
15 ] = 1.0 ; i = Jac -> size [ 0 ] * Jac -> size [ 1 ] ; Jac -> size [ 0 ] =
6 ; Jac -> size [ 1 ] = ( int32_T ) obj -> PositionNumber ; miyl1rq4uj ( Jac
, i ) ; loop_ub = 6 * ( int32_T ) obj -> PositionNumber ; if ( loop_ub - 1 >=
0 ) { memset ( & Jac -> data [ 0 ] , 0 , ( uint32_T ) loop_ub * sizeof (
real_T ) ) ; } c = kinematicPathIndices -> size [ 1 ] - 2 ; if (
kinematicPathIndices -> size [ 1 ] - 2 >= 0 ) { for ( i = 0 ; i < 5 ; i ++ )
{ b_p [ i ] = tmp_e [ i ] ; } } calykgtvmm ( & b , 2 ) ; calykgtvmm ( & tmp ,
2 ) ; for ( Jac_p = 0 ; Jac_p <= c ; Jac_p ++ ) { if ( kinematicPathIndices
-> data [ Jac_p ] != 0.0 ) { body1 = obj -> Bodies [ ( int32_T )
kinematicPathIndices -> data [ Jac_p ] - 1 ] ; } else { body1 = & obj -> Base
; } if ( kinematicPathIndices -> data [ Jac_p + 1 ] != 0.0 ) { body2 = obj ->
Bodies [ ( int32_T ) kinematicPathIndices -> data [ Jac_p + 1 ] - 1 ] ; }
else { body2 = & obj -> Base ; } nextBodyIsParent = ( body2 -> Index == body1
-> ParentIndex ) ; if ( nextBodyIsParent ) { body2 = body1 ; bid1 = 1.0 ; }
else { bid1 = - 1.0 ; } joint = body2 -> JointInternal ; i = body2Name ->
size [ 0 ] * body2Name -> size [ 1 ] ; body2Name -> size [ 0 ] = 1 ;
body2Name -> size [ 1 ] = joint -> Type -> size [ 1 ] ; e3n3mnwk4b (
body2Name , i ) ; loop_ub = joint -> Type -> size [ 1 ] ; for ( i = 0 ; i <
loop_ub ; i ++ ) { body2Name -> data [ i ] = joint -> Type -> data [ i ] ; }
b_bool = false ; if ( body2Name -> size [ 1 ] != 5 ) { } else { i = 1 ; do {
exitg1 = 0 ; if ( i - 1 < 5 ) { if ( body2Name -> data [ i - 1 ] != b_p [ i -
1 ] ) { exitg1 = 1 ; } else { i ++ ; } } else { b_bool = true ; exitg1 = 1 ;
} } while ( exitg1 == 0 ) ; } if ( b_bool ) { for ( i = 0 ; i < 16 ; i ++ ) {
Tj [ i ] = joint -> JointToParentTransform [ i ] ; } i = body2Name -> size [
0 ] * body2Name -> size [ 1 ] ; body2Name -> size [ 0 ] = 1 ; body2Name ->
size [ 1 ] = joint -> Type -> size [ 1 ] ; e3n3mnwk4b ( body2Name , i ) ;
loop_ub = joint -> Type -> size [ 1 ] ; for ( i = 0 ; i < loop_ub ; i ++ ) {
body2Name -> data [ i ] = joint -> Type -> data [ i ] ; } b_bool = false ; if
( body2Name -> size [ 1 ] != 5 ) { } else { i = 1 ; do { exitg1 = 0 ; if ( i
- 1 < 5 ) { if ( body2Name -> data [ i - 1 ] != b_p [ i - 1 ] ) { exitg1 = 1
; } else { i ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1
== 0 ) ; } if ( b_bool ) { bid2 = 0.0 ; } else { for ( i = 0 ; i < 8 ; i ++ )
{ b_i [ i ] = tmp_i [ i ] ; } if ( body2Name -> size [ 1 ] != 8 ) { } else {
i = 1 ; do { exitg1 = 0 ; if ( i - 1 < 8 ) { if ( body2Name -> data [ i - 1 ]
!= b_i [ i - 1 ] ) { exitg1 = 1 ; } else { i ++ ; } } else { b_bool = true ;
exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { bid2 = 1.0 ; }
else { bid2 = - 1.0 ; } } switch ( ( int32_T ) bid2 ) { case 0 : memset ( &
T1j [ 0 ] , 0 , sizeof ( real_T ) << 4U ) ; T1j [ 0 ] = 1.0 ; T1j [ 5 ] = 1.0
; T1j [ 10 ] = 1.0 ; T1j [ 15 ] = 1.0 ; break ; case 1 : cnixr4pd3ac ( joint
, v ) ; bid1 = v [ 0 ] ; qidx_idx_1 = v [ 1 ] ; axang_idx_2 = v [ 2 ] ; bid2
= 1.0 / muDoubleScalarSqrt ( ( bid1 * bid1 + qidx_idx_1 * qidx_idx_1 ) +
axang_idx_2 * axang_idx_2 ) ; v [ 0 ] = bid1 * bid2 ; v [ 1 ] = qidx_idx_1 *
bid2 ; v [ 2 ] = axang_idx_2 * bid2 ; lpd2uo4ns5 ( v [ 0 ] * v [ 0 ] * 0.0 +
1.0 , v [ 0 ] * v [ 1 ] * 0.0 - v [ 2 ] * 0.0 , v [ 0 ] * v [ 2 ] * 0.0 + v [
1 ] * 0.0 , v [ 0 ] * v [ 1 ] * 0.0 + v [ 2 ] * 0.0 , v [ 1 ] * v [ 1 ] * 0.0
+ 1.0 , v [ 1 ] * v [ 2 ] * 0.0 - v [ 0 ] * 0.0 , v [ 0 ] * v [ 2 ] * 0.0 - v
[ 1 ] * 0.0 , v [ 1 ] * v [ 2 ] * 0.0 + v [ 0 ] * 0.0 , v [ 2 ] * v [ 2 ] *
0.0 + 1.0 , tempR ) ; for ( g = 0 ; g < 3 ; g ++ ) { R [ g ] = tempR [ g * 3
] ; R [ g + 3 ] = tempR [ g * 3 + 1 ] ; R [ g + 6 ] = tempR [ g * 3 + 2 ] ; }
memset ( & T1j [ 0 ] , 0 , sizeof ( real_T ) << 4U ) ; for ( i = 0 ; i < 3 ;
i ++ ) { T1j [ i << 2 ] = R [ 3 * i ] ; T1j [ ( i << 2 ) + 1 ] = R [ 3 * i +
1 ] ; T1j [ ( i << 2 ) + 2 ] = R [ 3 * i + 2 ] ; } T1j [ 15 ] = 1.0 ; break ;
default : cnixr4pd3ac ( joint , v ) ; memset ( & tempR [ 0 ] , 0 , 9U *
sizeof ( real_T ) ) ; tempR [ 0 ] = 1.0 ; tempR [ 4 ] = 1.0 ; tempR [ 8 ] =
1.0 ; for ( i = 0 ; i < 3 ; i ++ ) { T1j [ i << 2 ] = tempR [ 3 * i ] ; T1j [
( i << 2 ) + 1 ] = tempR [ 3 * i + 1 ] ; T1j [ ( i << 2 ) + 2 ] = tempR [ 3 *
i + 2 ] ; T1j [ i + 12 ] = v [ i ] * 0.0 ; } T1j [ 3 ] = 0.0 ; T1j [ 7 ] =
0.0 ; T1j [ 11 ] = 0.0 ; T1j [ 15 ] = 1.0 ; break ; } for ( i = 0 ; i < 16 ;
i ++ ) { b_e [ i ] = joint -> ChildToJointTransform [ i ] ; } for ( i = 0 ; i
< 4 ; i ++ ) { for ( f = 0 ; f < 4 ; f ++ ) { Tj_p [ i + ( f << 2 ) ] = 0.0 ;
Tj_p [ i + ( f << 2 ) ] += T1j [ f << 2 ] * Tj [ i ] ; Tj_p [ i + ( f << 2 )
] += T1j [ ( f << 2 ) + 1 ] * Tj [ i + 4 ] ; Tj_p [ i + ( f << 2 ) ] += T1j [
( f << 2 ) + 2 ] * Tj [ i + 8 ] ; Tj_p [ i + ( f << 2 ) ] += T1j [ ( f << 2 )
+ 3 ] * Tj [ i + 12 ] ; } for ( f = 0 ; f < 4 ; f ++ ) { Tc2p [ i + ( f << 2
) ] = 0.0 ; Tc2p [ i + ( f << 2 ) ] += b_e [ f << 2 ] * Tj_p [ i ] ; Tc2p [ i
+ ( f << 2 ) ] += b_e [ ( f << 2 ) + 1 ] * Tj_p [ i + 4 ] ; Tc2p [ i + ( f <<
2 ) ] += b_e [ ( f << 2 ) + 2 ] * Tj_p [ i + 8 ] ; Tc2p [ i + ( f << 2 ) ] +=
b_e [ ( f << 2 ) + 3 ] * Tj_p [ i + 12 ] ; } } } else { i = ( int32_T ) body2
-> Index ; bid2 = obj -> PositionDoFMap [ i - 1 ] ; qidx_idx_1 = obj ->
PositionDoFMap [ i + 8 ] ; if ( bid2 > qidx_idx_1 ) { g = 0 ; f = 0 ; } else
{ g = ( int32_T ) bid2 - 1 ; f = ( int32_T ) qidx_idx_1 ; } for ( i = 0 ; i <
16 ; i ++ ) { Tj [ i ] = joint -> JointToParentTransform [ i ] ; } i =
body2Name -> size [ 0 ] * body2Name -> size [ 1 ] ; body2Name -> size [ 0 ] =
1 ; body2Name -> size [ 1 ] = joint -> Type -> size [ 1 ] ; e3n3mnwk4b (
body2Name , i ) ; loop_ub = joint -> Type -> size [ 1 ] ; for ( i = 0 ; i <
loop_ub ; i ++ ) { body2Name -> data [ i ] = joint -> Type -> data [ i ] ; }
if ( body2Name -> size [ 1 ] != 5 ) { } else { i = 1 ; do { exitg1 = 0 ; if (
i - 1 < 5 ) { if ( body2Name -> data [ i - 1 ] != b_p [ i - 1 ] ) { exitg1 =
1 ; } else { i ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { bid2 = 0.0 ; } else { for ( i = 0 ; i < 8 ;
i ++ ) { b_i [ i ] = tmp_i [ i ] ; } if ( body2Name -> size [ 1 ] != 8 ) { }
else { i = 1 ; do { exitg1 = 0 ; if ( i - 1 < 8 ) { if ( body2Name -> data [
i - 1 ] != b_i [ i - 1 ] ) { exitg1 = 1 ; } else { i ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { bid2 = 1.0
; } else { bid2 = - 1.0 ; } } switch ( ( int32_T ) bid2 ) { case 0 : memset (
& T1j [ 0 ] , 0 , sizeof ( real_T ) << 4U ) ; T1j [ 0 ] = 1.0 ; T1j [ 5 ] =
1.0 ; T1j [ 10 ] = 1.0 ; T1j [ 15 ] = 1.0 ; break ; case 1 : cnixr4pd3ac (
joint , v ) ; result_data [ 0 ] = v [ 0 ] ; result_data [ 1 ] = v [ 1 ] ;
result_data [ 2 ] = v [ 2 ] ; if ( ( f - g != 0 ) - 1 >= 0 ) { result_data [
3 ] = qv [ g ] ; } bid2 = result_data [ 0 ] ; v [ 0 ] = bid2 * bid2 ; bid2 =
result_data [ 1 ] ; v [ 1 ] = bid2 * bid2 ; bid2 = result_data [ 2 ] ; bid2 =
1.0 / muDoubleScalarSqrt ( ( v [ 0 ] + v [ 1 ] ) + bid2 * bid2 ) ; v [ 0 ] =
result_data [ 0 ] * bid2 ; v [ 1 ] = result_data [ 1 ] * bid2 ; v [ 2 ] =
result_data [ 2 ] * bid2 ; qidx_idx_1 = result_data [ 3 ] ; bid2 =
muDoubleScalarCos ( qidx_idx_1 ) ; qidx_idx_1 = muDoubleScalarSin (
qidx_idx_1 ) ; lpd2uo4ns5 ( v [ 0 ] * v [ 0 ] * ( 1.0 - bid2 ) + bid2 , v [ 0
] * v [ 1 ] * ( 1.0 - bid2 ) - v [ 2 ] * qidx_idx_1 , v [ 0 ] * v [ 2 ] * (
1.0 - bid2 ) + v [ 1 ] * qidx_idx_1 , v [ 0 ] * v [ 1 ] * ( 1.0 - bid2 ) + v
[ 2 ] * qidx_idx_1 , v [ 1 ] * v [ 1 ] * ( 1.0 - bid2 ) + bid2 , v [ 1 ] * v
[ 2 ] * ( 1.0 - bid2 ) - v [ 0 ] * qidx_idx_1 , v [ 0 ] * v [ 2 ] * ( 1.0 -
bid2 ) - v [ 1 ] * qidx_idx_1 , v [ 1 ] * v [ 2 ] * ( 1.0 - bid2 ) + v [ 0 ]
* qidx_idx_1 , v [ 2 ] * v [ 2 ] * ( 1.0 - bid2 ) + bid2 , tempR ) ; for ( g
= 0 ; g < 3 ; g ++ ) { R [ g ] = tempR [ g * 3 ] ; R [ g + 3 ] = tempR [ g *
3 + 1 ] ; R [ g + 6 ] = tempR [ g * 3 + 2 ] ; } memset ( & T1j [ 0 ] , 0 ,
sizeof ( real_T ) << 4U ) ; for ( i = 0 ; i < 3 ; i ++ ) { T1j [ i << 2 ] = R
[ 3 * i ] ; T1j [ ( i << 2 ) + 1 ] = R [ 3 * i + 1 ] ; T1j [ ( i << 2 ) + 2 ]
= R [ 3 * i + 2 ] ; } T1j [ 15 ] = 1.0 ; break ; default : cnixr4pd3ac (
joint , v ) ; memset ( & tempR [ 0 ] , 0 , 9U * sizeof ( real_T ) ) ; tempR [
0 ] = 1.0 ; tempR [ 4 ] = 1.0 ; tempR [ 8 ] = 1.0 ; bid2 = qv [ g ] ; for ( i
= 0 ; i < 3 ; i ++ ) { T1j [ i << 2 ] = tempR [ 3 * i ] ; T1j [ ( i << 2 ) +
1 ] = tempR [ 3 * i + 1 ] ; T1j [ ( i << 2 ) + 2 ] = tempR [ 3 * i + 2 ] ;
T1j [ i + 12 ] = v [ i ] * bid2 ; } T1j [ 3 ] = 0.0 ; T1j [ 7 ] = 0.0 ; T1j [
11 ] = 0.0 ; T1j [ 15 ] = 1.0 ; break ; } for ( i = 0 ; i < 16 ; i ++ ) { b_e
[ i ] = joint -> ChildToJointTransform [ i ] ; } for ( i = 0 ; i < 4 ; i ++ )
{ for ( f = 0 ; f < 4 ; f ++ ) { Tj_p [ i + ( f << 2 ) ] = 0.0 ; Tj_p [ i + (
f << 2 ) ] += T1j [ f << 2 ] * Tj [ i ] ; Tj_p [ i + ( f << 2 ) ] += T1j [ (
f << 2 ) + 1 ] * Tj [ i + 4 ] ; Tj_p [ i + ( f << 2 ) ] += T1j [ ( f << 2 ) +
2 ] * Tj [ i + 8 ] ; Tj_p [ i + ( f << 2 ) ] += T1j [ ( f << 2 ) + 3 ] * Tj [
i + 12 ] ; } for ( f = 0 ; f < 4 ; f ++ ) { Tc2p [ i + ( f << 2 ) ] = 0.0 ;
Tc2p [ i + ( f << 2 ) ] += b_e [ f << 2 ] * Tj_p [ i ] ; Tc2p [ i + ( f << 2
) ] += b_e [ ( f << 2 ) + 1 ] * Tj_p [ i + 4 ] ; Tc2p [ i + ( f << 2 ) ] +=
b_e [ ( f << 2 ) + 2 ] * Tj_p [ i + 8 ] ; Tc2p [ i + ( f << 2 ) ] += b_e [ (
f << 2 ) + 3 ] * Tj_p [ i + 12 ] ; } } i = ( int32_T ) body2 -> Index ; bid2
= obj -> VelocityDoFMap [ i - 1 ] ; qidx_idx_1 = obj -> VelocityDoFMap [ i +
8 ] ; if ( nextBodyIsParent ) { for ( i = 0 ; i < 16 ; i ++ ) { Tj [ i ] =
joint -> ChildToJointTransform [ i ] ; } } else { for ( i = 0 ; i < 16 ; i ++
) { T1j [ i ] = joint -> JointToParentTransform [ i ] ; } for ( i = 0 ; i < 3
; i ++ ) { R [ 3 * i ] = T1j [ i ] ; R [ 3 * i + 1 ] = T1j [ i + 4 ] ; R [ 3
* i + 2 ] = T1j [ i + 8 ] ; } for ( i = 0 ; i < 9 ; i ++ ) { tempR [ i ] = -
R [ i ] ; } for ( i = 0 ; i < 3 ; i ++ ) { Tj [ i << 2 ] = R [ 3 * i ] ; Tj [
( i << 2 ) + 1 ] = R [ 3 * i + 1 ] ; Tj [ ( i << 2 ) + 2 ] = R [ 3 * i + 2 ]
; Tj [ i + 12 ] = ( tempR [ i + 3 ] * T1j [ 13 ] + tempR [ i ] * T1j [ 12 ] )
+ tempR [ i + 6 ] * T1j [ 14 ] ; } Tj [ 3 ] = 0.0 ; Tj [ 7 ] = 0.0 ; Tj [ 11
] = 0.0 ; Tj [ 15 ] = 1.0 ; } for ( i = 0 ; i < 4 ; i ++ ) { for ( f = 0 ; f
< 4 ; f ++ ) { T1j [ i + ( f << 2 ) ] = 0.0 ; T1j [ i + ( f << 2 ) ] += T1 [
f << 2 ] * Tj [ i ] ; T1j [ i + ( f << 2 ) ] += T1 [ ( f << 2 ) + 1 ] * Tj [
i + 4 ] ; T1j [ i + ( f << 2 ) ] += T1 [ ( f << 2 ) + 2 ] * Tj [ i + 8 ] ;
T1j [ i + ( f << 2 ) ] += T1 [ ( f << 2 ) + 3 ] * Tj [ i + 12 ] ; } } for ( i
= 0 ; i < 3 ; i ++ ) { R [ 3 * i ] = T1j [ i ] ; R [ 3 * i + 1 ] = T1j [ i +
4 ] ; R [ 3 * i + 2 ] = T1j [ i + 8 ] ; } for ( i = 0 ; i < 9 ; i ++ ) {
tempR [ i ] = - R [ i ] ; } for ( i = 0 ; i < 3 ; i ++ ) { Tj [ i << 2 ] = R
[ 3 * i ] ; Tj [ ( i << 2 ) + 1 ] = R [ 3 * i + 1 ] ; Tj [ ( i << 2 ) + 2 ] =
R [ 3 * i + 2 ] ; Tj [ i + 12 ] = ( tempR [ i + 3 ] * T1j [ 13 ] + tempR [ i
] * T1j [ 12 ] ) + tempR [ i + 6 ] * T1j [ 14 ] ; } Tj [ 3 ] = 0.0 ; Tj [ 7 ]
= 0.0 ; Tj [ 11 ] = 0.0 ; Tj [ 15 ] = 1.0 ; i = b -> size [ 0 ] * b -> size [
1 ] ; b -> size [ 0 ] = 6 ; b -> size [ 1 ] = joint -> MotionSubspace -> size
[ 1 ] ; miyl1rq4uj ( b , i ) ; loop_ub = 6 * joint -> MotionSubspace -> size
[ 1 ] ; for ( i = 0 ; i < loop_ub ; i ++ ) { b -> data [ i ] = joint ->
MotionSubspace -> data [ i ] ; } if ( bid2 > qidx_idx_1 ) { g = 0 ; } else {
g = ( int32_T ) bid2 - 1 ; } R [ 0 ] = 0.0 ; R [ 3 ] = - Tj [ 14 ] ; R [ 6 ]
= Tj [ 13 ] ; R [ 1 ] = Tj [ 14 ] ; R [ 4 ] = 0.0 ; R [ 7 ] = - Tj [ 12 ] ; R
[ 2 ] = - Tj [ 13 ] ; R [ 5 ] = Tj [ 12 ] ; R [ 8 ] = 0.0 ; for ( i = 0 ; i <
3 ; i ++ ) { for ( f = 0 ; f < 3 ; f ++ ) { tempR [ i + 3 * f ] = 0.0 ; tempR
[ i + 3 * f ] += Tj [ f << 2 ] * R [ i ] ; tempR [ i + 3 * f ] += Tj [ ( f <<
2 ) + 1 ] * R [ i + 3 ] ; tempR [ i + 3 * f ] += Tj [ ( f << 2 ) + 2 ] * R [
i + 6 ] ; Tj_e [ f + 6 * i ] = Tj [ ( i << 2 ) + f ] ; Tj_e [ f + 6 * ( i + 3
) ] = 0.0 ; } } for ( i = 0 ; i < 3 ; i ++ ) { Tj_e [ 6 * i + 3 ] = tempR [ 3
* i ] ; Tj_e [ 6 * ( i + 3 ) + 3 ] = Tj [ i << 2 ] ; Tj_e [ 6 * i + 4 ] =
tempR [ 3 * i + 1 ] ; Tj_e [ 6 * ( i + 3 ) + 4 ] = Tj [ ( i << 2 ) + 1 ] ;
Tj_e [ 6 * i + 5 ] = tempR [ 3 * i + 2 ] ; Tj_e [ 6 * ( i + 3 ) + 5 ] = Tj [
( i << 2 ) + 2 ] ; } l5ks21uytp ( Tj_e , b , tmp ) ; loop_ub = tmp -> size [
1 ] ; for ( i = 0 ; i < loop_ub ; i ++ ) { for ( f = 0 ; f <= 4 ; f += 2 ) {
tmp_p = _mm_loadu_pd ( & tmp -> data [ 6 * i + f ] ) ; _mm_storeu_pd ( & Jac
-> data [ f + 6 * ( g + i ) ] , _mm_mul_pd ( tmp_p , _mm_set1_pd ( bid1 ) ) )
; } } } if ( nextBodyIsParent ) { for ( i = 0 ; i < 4 ; i ++ ) { for ( f = 0
; f < 4 ; f ++ ) { Tj [ i + ( f << 2 ) ] = 0.0 ; Tj [ i + ( f << 2 ) ] += T1
[ f << 2 ] * Tc2p [ i ] ; Tj [ i + ( f << 2 ) ] += T1 [ ( f << 2 ) + 1 ] *
Tc2p [ i + 4 ] ; Tj [ i + ( f << 2 ) ] += T1 [ ( f << 2 ) + 2 ] * Tc2p [ i +
8 ] ; Tj [ i + ( f << 2 ) ] += T1 [ ( f << 2 ) + 3 ] * Tc2p [ i + 12 ] ; } }
memcpy ( & T1 [ 0 ] , & Tj [ 0 ] , sizeof ( real_T ) << 4U ) ; } else { for (
i = 0 ; i < 3 ; i ++ ) { R [ 3 * i ] = Tc2p [ i ] ; R [ 3 * i + 1 ] = Tc2p [
i + 4 ] ; R [ 3 * i + 2 ] = Tc2p [ i + 8 ] ; } for ( i = 0 ; i < 9 ; i ++ ) {
tempR [ i ] = - R [ i ] ; } for ( i = 0 ; i < 3 ; i ++ ) { Tj [ i << 2 ] = R
[ 3 * i ] ; Tj [ ( i << 2 ) + 1 ] = R [ 3 * i + 1 ] ; Tj [ ( i << 2 ) + 2 ] =
R [ 3 * i + 2 ] ; Tj [ i + 12 ] = ( tempR [ i + 3 ] * Tc2p [ 13 ] + tempR [ i
] * Tc2p [ 12 ] ) + tempR [ i + 6 ] * Tc2p [ 14 ] ; } Tj [ 3 ] = 0.0 ; Tj [ 7
] = 0.0 ; Tj [ 11 ] = 0.0 ; Tj [ 15 ] = 1.0 ; for ( i = 0 ; i < 4 ; i ++ ) {
for ( f = 0 ; f < 4 ; f ++ ) { Tc2p [ i + ( f << 2 ) ] = 0.0 ; Tc2p [ i + ( f
<< 2 ) ] += T1 [ f << 2 ] * Tj [ i ] ; Tc2p [ i + ( f << 2 ) ] += T1 [ ( f <<
2 ) + 1 ] * Tj [ i + 4 ] ; Tc2p [ i + ( f << 2 ) ] += T1 [ ( f << 2 ) + 2 ] *
Tj [ i + 8 ] ; Tc2p [ i + ( f << 2 ) ] += T1 [ ( f << 2 ) + 3 ] * Tj [ i + 12
] ; } } memcpy ( & T1 [ 0 ] , & Tc2p [ 0 ] , sizeof ( real_T ) << 4U ) ; } }
bdhoypqe5x ( & tmp ) ; bdhoypqe5x ( & b ) ; fagcrr3avn ( & body2Name ) ;
bdhoypqe5x ( & kinematicPathIndices ) ; for ( i = 0 ; i < 3 ; i ++ ) { bid1 =
T1 [ i << 2 ] ; Tj_e [ 6 * i ] = bid1 ; Tj_e [ 6 * ( i + 3 ) ] = 0.0 ; Tj_e [
6 * i + 3 ] = 0.0 ; Tj_e [ 6 * ( i + 3 ) + 3 ] = bid1 ; bid1 = T1 [ ( i << 2
) + 1 ] ; Tj_e [ 6 * i + 1 ] = bid1 ; Tj_e [ 6 * ( i + 3 ) + 1 ] = 0.0 ; Tj_e
[ 6 * i + 4 ] = 0.0 ; Tj_e [ 6 * ( i + 3 ) + 4 ] = bid1 ; bid1 = T1 [ ( i <<
2 ) + 2 ] ; Tj_e [ 6 * i + 2 ] = bid1 ; Tj_e [ 6 * ( i + 3 ) + 2 ] = 0.0 ;
Tj_e [ 6 * i + 5 ] = 0.0 ; Tj_e [ 6 * ( i + 3 ) + 5 ] = bid1 ; } calykgtvmm (
& Jac_e , 2 ) ; i = Jac_e -> size [ 0 ] * Jac_e -> size [ 1 ] ; Jac_e -> size
[ 0 ] = 6 ; Jac_e -> size [ 1 ] = Jac -> size [ 1 ] ; miyl1rq4uj ( Jac_e , i
) ; loop_ub = Jac -> size [ 0 ] * Jac -> size [ 1 ] - 1 ; if ( loop_ub >= 0 )
{ memcpy ( & Jac_e -> data [ 0 ] , & Jac -> data [ 0 ] , ( uint32_T ) (
loop_ub + 1 ) * sizeof ( real_T ) ) ; } l5ks21uytp ( Tj_e , Jac_e , Jac ) ;
bdhoypqe5x ( & Jac_e ) ; T_size [ 0 ] = 4 ; T_size [ 1 ] = 4 ; memcpy ( &
T_data [ 0 ] , & T1 [ 0 ] , sizeof ( real_T ) << 4U ) ; } static creal_T
hv02b2lq4w ( const creal_T x ) { creal_T b_x ; real_T absxr ; real_T xr ; xr
= x . re ; if ( x . im == 0.0 ) { if ( x . re < 0.0 ) { absxr = 0.0 ; xr =
muDoubleScalarSqrt ( - x . re ) ; } else { absxr = muDoubleScalarSqrt ( x .
re ) ; xr = 0.0 ; } } else if ( x . re == 0.0 ) { if ( x . im < 0.0 ) { absxr
= muDoubleScalarSqrt ( - x . im / 2.0 ) ; xr = - absxr ; } else { absxr =
muDoubleScalarSqrt ( x . im / 2.0 ) ; xr = absxr ; } } else if (
muDoubleScalarIsNaN ( x . re ) ) { absxr = ( rtNaN ) ; } else if (
muDoubleScalarIsNaN ( x . im ) ) { absxr = ( rtNaN ) ; xr = ( rtNaN ) ; }
else if ( muDoubleScalarIsInf ( x . im ) ) { absxr = muDoubleScalarAbs ( x .
im ) ; xr = x . im ; } else if ( muDoubleScalarIsInf ( x . re ) ) { if ( x .
re < 0.0 ) { absxr = 0.0 ; xr = x . im * - x . re ; } else { absxr = x . re ;
xr = 0.0 ; } } else { absxr = muDoubleScalarAbs ( x . re ) ; xr =
muDoubleScalarAbs ( x . im ) ; if ( ( absxr > 4.4942328371557893E+307 ) || (
xr > 4.4942328371557893E+307 ) ) { absxr *= 0.5 ; xr *= 0.5 ; xr =
muDoubleScalarHypot ( absxr , xr ) ; if ( xr > absxr ) { absxr =
muDoubleScalarSqrt ( absxr / xr + 1.0 ) * muDoubleScalarSqrt ( xr ) ; } else
{ absxr = muDoubleScalarSqrt ( xr ) * 1.4142135623730951 ; } } else { absxr =
muDoubleScalarSqrt ( ( muDoubleScalarHypot ( absxr , xr ) + absxr ) * 0.5 ) ;
} if ( x . re > 0.0 ) { xr = x . im / absxr * 0.5 ; } else { if ( x . im <
0.0 ) { xr = - absxr ; } else { xr = absxr ; } absxr = x . im / xr * 0.5 ; }
} b_x . re = absxr ; b_x . im = xr ; return b_x ; } static real_T iq4v5bez02
( int32_T n , const real_T x [ 9 ] , int32_T ix0 ) { real_T absxk ; real_T
scale ; real_T t ; real_T y ; int32_T k ; int32_T kend ; y = 0.0 ; scale =
3.3121686421112381E-170 ; kend = ix0 + n ; for ( k = ix0 ; k < kend ; k ++ )
{ absxk = muDoubleScalarAbs ( x [ k - 1 ] ) ; if ( absxk > scale ) { t =
scale / absxk ; y = y * t * t + 1.0 ; scale = absxk ; } else { t = absxk /
scale ; y += t * t ; } } return scale * muDoubleScalarSqrt ( y ) ; } static
real_T hxwoj44b1k ( int32_T n , const real_T x [ 9 ] , int32_T ix0 , const
real_T y [ 9 ] , int32_T iy0 ) { real_T d ; int32_T b ; int32_T k ; d = 0.0 ;
b = ( uint8_T ) n ; for ( k = 0 ; k < b ; k ++ ) { d += x [ ( ix0 + k ) - 1 ]
* y [ ( iy0 + k ) - 1 ] ; } return d ; } static void dqzyu4u4dr ( int32_T n ,
real_T a , int32_T ix0 , const real_T y [ 9 ] , int32_T iy0 , real_T b_y [ 9
] ) { int32_T k ; memcpy ( & b_y [ 0 ] , & y [ 0 ] , 9U * sizeof ( real_T ) )
; if ( ! ( a == 0.0 ) ) { for ( k = 0 ; k < n ; k ++ ) { b_y [ ( iy0 + k ) -
1 ] += b_y [ ( ix0 + k ) - 1 ] * a ; } } } static real_T iq4v5bez02k ( const
real_T x [ 3 ] , int32_T ix0 ) { real_T absxk ; real_T scale ; real_T t ;
real_T y ; int32_T k ; y = 0.0 ; scale = 3.3121686421112381E-170 ; for ( k =
ix0 ; k <= ix0 + 1 ; k ++ ) { absxk = muDoubleScalarAbs ( x [ k - 1 ] ) ; if
( absxk > scale ) { t = scale / absxk ; y = y * t * t + 1.0 ; scale = absxk ;
} else { t = absxk / scale ; y += t * t ; } } return scale *
muDoubleScalarSqrt ( y ) ; } static void dqzyu4u4dryag ( int32_T n , real_T a
, const real_T x [ 9 ] , int32_T ix0 , real_T y [ 3 ] , int32_T iy0 ) {
__m128d tmp ; int32_T k ; int32_T scalarLB ; int32_T vectorUB ; if ( ! ( a ==
0.0 ) ) { scalarLB = ( n / 2 ) << 1 ; vectorUB = scalarLB - 2 ; for ( k = 0 ;
k <= vectorUB ; k += 2 ) { tmp = _mm_loadu_pd ( & y [ ( iy0 + k ) - 1 ] ) ;
_mm_storeu_pd ( & y [ ( iy0 + k ) - 1 ] , _mm_add_pd ( _mm_mul_pd (
_mm_loadu_pd ( & x [ ( ix0 + k ) - 1 ] ) , _mm_set1_pd ( a ) ) , tmp ) ) ; }
for ( k = scalarLB ; k < n ; k ++ ) { y [ ( iy0 + k ) - 1 ] += x [ ( ix0 + k
) - 1 ] * a ; } } } static void dqzyu4u4drya ( int32_T n , real_T a , const
real_T x [ 3 ] , int32_T ix0 , const real_T y [ 9 ] , int32_T iy0 , real_T
b_y [ 9 ] ) { __m128d tmp ; int32_T k ; int32_T scalarLB ; int32_T vectorUB ;
memcpy ( & b_y [ 0 ] , & y [ 0 ] , 9U * sizeof ( real_T ) ) ; if ( ! ( a ==
0.0 ) ) { scalarLB = ( n / 2 ) << 1 ; vectorUB = scalarLB - 2 ; for ( k = 0 ;
k <= vectorUB ; k += 2 ) { tmp = _mm_loadu_pd ( & b_y [ ( iy0 + k ) - 1 ] ) ;
_mm_storeu_pd ( & b_y [ ( iy0 + k ) - 1 ] , _mm_add_pd ( _mm_mul_pd (
_mm_loadu_pd ( & x [ ( ix0 + k ) - 1 ] ) , _mm_set1_pd ( a ) ) , tmp ) ) ; }
for ( k = scalarLB ; k < n ; k ++ ) { b_y [ ( iy0 + k ) - 1 ] += x [ ( ix0 +
k ) - 1 ] * a ; } } } static void h1cxhqfs1e ( const real_T x [ 9 ] , int32_T
ix0 , int32_T iy0 , real_T b_x [ 9 ] ) { real_T temp ; memcpy ( & b_x [ 0 ] ,
& x [ 0 ] , 9U * sizeof ( real_T ) ) ; temp = b_x [ ix0 - 1 ] ; b_x [ ix0 - 1
] = b_x [ iy0 - 1 ] ; b_x [ iy0 - 1 ] = temp ; temp = b_x [ ix0 ] ; b_x [ ix0
] = b_x [ iy0 ] ; b_x [ iy0 ] = temp ; temp = b_x [ ix0 + 1 ] ; b_x [ ix0 + 1
] = b_x [ iy0 + 1 ] ; b_x [ iy0 + 1 ] = temp ; } static void pwomuwmg0o (
real_T a , real_T b , real_T * b_a , real_T * b_b , real_T * c , real_T * s )
{ real_T absa ; real_T absb ; real_T ads ; real_T bds ; real_T roe ; real_T
scale ; roe = b ; absa = muDoubleScalarAbs ( a ) ; absb = muDoubleScalarAbs (
b ) ; if ( absa > absb ) { roe = a ; } scale = absa + absb ; if ( scale ==
0.0 ) { * s = 0.0 ; * c = 1.0 ; * b_a = 0.0 ; * b_b = 0.0 ; } else { ads =
absa / scale ; bds = absb / scale ; * b_a = muDoubleScalarSqrt ( ads * ads +
bds * bds ) * scale ; if ( roe < 0.0 ) { * b_a = - * b_a ; } * c = a / * b_a
; * s = b / * b_a ; if ( absa > absb ) { * b_b = * s ; } else if ( * c != 0.0
) { * b_b = 1.0 / * c ; } else { * b_b = 1.0 ; } } } static void jqns5sfonc (
const real_T x [ 9 ] , int32_T ix0 , int32_T iy0 , real_T c , real_T s ,
real_T b_x [ 9 ] ) { real_T temp ; memcpy ( & b_x [ 0 ] , & x [ 0 ] , 9U *
sizeof ( real_T ) ) ; temp = b_x [ ix0 - 1 ] * c + b_x [ iy0 - 1 ] * s ; b_x
[ iy0 - 1 ] = b_x [ iy0 - 1 ] * c - b_x [ ix0 - 1 ] * s ; b_x [ ix0 - 1 ] =
temp ; temp = b_x [ ix0 ] * c + b_x [ iy0 ] * s ; b_x [ iy0 ] = b_x [ iy0 ] *
c - b_x [ ix0 ] * s ; b_x [ ix0 ] = temp ; temp = b_x [ ix0 + 1 ] * c + b_x [
iy0 + 1 ] * s ; b_x [ iy0 + 1 ] = b_x [ iy0 + 1 ] * c - b_x [ ix0 + 1 ] * s ;
b_x [ ix0 + 1 ] = temp ; } static void o5vuefzdbw ( const real_T A [ 9 ] ,
real_T U [ 9 ] , real_T s [ 3 ] , real_T V [ 9 ] ) { __m128d tmp ; real_T A_e
[ 9 ] ; real_T A_p [ 9 ] ; real_T e [ 3 ] ; real_T s_p [ 3 ] ; real_T work [
3 ] ; real_T emm1 ; real_T nrm ; real_T rt ; real_T shift ; real_T smm1 ;
real_T sqds ; real_T ztest ; real_T ztest0 ; int32_T b ; int32_T colqp1 ;
int32_T m ; int32_T qjj ; int32_T qp1 ; int32_T qq ; int32_T scalarLB ;
int32_T vectorUB ; boolean_T apply_transform ; boolean_T exitg1 ; s_p [ 0 ] =
0.0 ; e [ 0 ] = 0.0 ; work [ 0 ] = 0.0 ; s_p [ 1 ] = 0.0 ; e [ 1 ] = 0.0 ;
work [ 1 ] = 0.0 ; s_p [ 2 ] = 0.0 ; e [ 2 ] = 0.0 ; work [ 2 ] = 0.0 ; for (
m = 0 ; m < 9 ; m ++ ) { A_p [ m ] = A [ m ] ; U [ m ] = 0.0 ; V [ m ] = 0.0
; } for ( m = 0 ; m < 2 ; m ++ ) { colqp1 = m + 1 ; qp1 = m + 2 ; qq = ( m *
3 + m ) + 1 ; apply_transform = false ; nrm = iq4v5bez02 ( 3 - m , A_p , qq )
; if ( nrm > 0.0 ) { apply_transform = true ; if ( A_p [ qq - 1 ] < 0.0 ) {
s_p [ m ] = - nrm ; } else { s_p [ m ] = nrm ; } if ( muDoubleScalarAbs ( s_p
[ m ] ) >= 1.0020841800044864E-292 ) { nrm = 1.0 / s_p [ m ] ; b = ( qq - m )
- 1 ; scalarLB = ( ( ( ( b - qq ) + 4 ) / 2 ) << 1 ) + qq ; vectorUB =
scalarLB - 2 ; for ( qjj = qq ; qjj <= vectorUB ; qjj += 2 ) { tmp =
_mm_loadu_pd ( & A_p [ qjj - 1 ] ) ; _mm_storeu_pd ( & A_p [ qjj - 1 ] ,
_mm_mul_pd ( tmp , _mm_set1_pd ( nrm ) ) ) ; } for ( qjj = scalarLB ; qjj <=
b + 3 ; qjj ++ ) { A_p [ qjj - 1 ] *= nrm ; } } else { b = ( qq - m ) - 1 ;
scalarLB = ( ( ( ( b - qq ) + 4 ) / 2 ) << 1 ) + qq ; vectorUB = scalarLB - 2
; for ( qjj = qq ; qjj <= vectorUB ; qjj += 2 ) { tmp = _mm_loadu_pd ( & A_p
[ qjj - 1 ] ) ; _mm_storeu_pd ( & A_p [ qjj - 1 ] , _mm_div_pd ( tmp ,
_mm_set1_pd ( s_p [ m ] ) ) ) ; } for ( qjj = scalarLB ; qjj <= b + 3 ; qjj
++ ) { A_p [ qjj - 1 ] /= s_p [ m ] ; } } A_p [ qq - 1 ] ++ ; s_p [ m ] = -
s_p [ m ] ; } else { s_p [ m ] = 0.0 ; } for ( b = qp1 ; b < 4 ; b ++ ) { qjj
= ( ( b - 1 ) * 3 + m ) + 1 ; if ( apply_transform ) { memcpy ( & A_e [ 0 ] ,
& A_p [ 0 ] , 9U * sizeof ( real_T ) ) ; dqzyu4u4dr ( 3 - m , - ( hxwoj44b1k
( 3 - m , A_p , qq , A_p , qjj ) / A_p [ m * 3 + m ] ) , qq , A_e , qjj , A_p
) ; } e [ b - 1 ] = A_p [ qjj - 1 ] ; } memcpy ( & U [ ( m * 3 + colqp1 ) + -
1 ] , & A_p [ ( m * 3 + colqp1 ) + - 1 ] , ( uint32_T ) ( - colqp1 + 4 ) *
sizeof ( real_T ) ) ; if ( m + 1 <= 1 ) { nrm = iq4v5bez02k ( e , 2 ) ; if (
nrm == 0.0 ) { e [ 0 ] = 0.0 ; } else { if ( e [ 1 ] < 0.0 ) { rt = - nrm ; e
[ 0 ] = - nrm ; } else { rt = nrm ; e [ 0 ] = nrm ; } if ( muDoubleScalarAbs
( rt ) >= 1.0020841800044864E-292 ) { nrm = 1.0 / rt ; scalarLB = ( ( ( ( 2 -
m ) / 2 ) << 1 ) + m ) + 2 ; vectorUB = scalarLB - 2 ; for ( qjj = qp1 ; qjj
<= vectorUB ; qjj += 2 ) { tmp = _mm_loadu_pd ( & e [ qjj - 1 ] ) ;
_mm_storeu_pd ( & e [ qjj - 1 ] , _mm_mul_pd ( tmp , _mm_set1_pd ( nrm ) ) )
; } for ( qjj = scalarLB ; qjj < 4 ; qjj ++ ) { e [ qjj - 1 ] *= nrm ; } }
else { scalarLB = ( ( ( ( 2 - m ) / 2 ) << 1 ) + m ) + 2 ; vectorUB =
scalarLB - 2 ; for ( qjj = qp1 ; qjj <= vectorUB ; qjj += 2 ) { tmp =
_mm_loadu_pd ( & e [ qjj - 1 ] ) ; _mm_storeu_pd ( & e [ qjj - 1 ] ,
_mm_div_pd ( tmp , _mm_set1_pd ( rt ) ) ) ; } for ( qjj = scalarLB ; qjj < 4
; qjj ++ ) { e [ qjj - 1 ] /= rt ; } } e [ 1 ] ++ ; e [ 0 ] = - e [ 0 ] ; for
( qq = qp1 ; qq < 4 ; qq ++ ) { work [ qq - 1 ] = 0.0 ; } for ( qq = qp1 ; qq
< 4 ; qq ++ ) { dqzyu4u4dryag ( 2 , e [ qq - 1 ] , A_p , 3 * ( qq - 1 ) + 2 ,
work , 2 ) ; } for ( qq = qp1 ; qq < 4 ; qq ++ ) { memcpy ( & A_e [ 0 ] , &
A_p [ 0 ] , 9U * sizeof ( real_T ) ) ; dqzyu4u4drya ( 2 , - e [ qq - 1 ] / e
[ 1 ] , work , 2 , A_e , ( qq - 1 ) * 3 + 2 , A_p ) ; } } for ( colqp1 = qp1
; colqp1 < 4 ; colqp1 ++ ) { V [ colqp1 - 1 ] = e [ colqp1 - 1 ] ; } } } m =
2 ; s_p [ 2 ] = A_p [ 8 ] ; e [ 1 ] = A_p [ 7 ] ; e [ 2 ] = 0.0 ; U [ 6 ] =
0.0 ; U [ 7 ] = 0.0 ; U [ 8 ] = 1.0 ; for ( colqp1 = 1 ; colqp1 >= 0 ; colqp1
-- ) { qq = 3 * colqp1 + colqp1 ; if ( s_p [ colqp1 ] != 0.0 ) { for ( b =
colqp1 + 2 ; b < 4 ; b ++ ) { qjj = ( ( b - 1 ) * 3 + colqp1 ) + 1 ; memcpy (
& A_p [ 0 ] , & U [ 0 ] , 9U * sizeof ( real_T ) ) ; dqzyu4u4dr ( 3 - colqp1
, - ( hxwoj44b1k ( 3 - colqp1 , U , qq + 1 , U , qjj ) / U [ qq ] ) , qq + 1
, A_p , qjj , U ) ; } for ( qp1 = colqp1 + 1 ; qp1 < 4 ; qp1 ++ ) { U [ ( qp1
+ 3 * colqp1 ) - 1 ] = - U [ ( 3 * colqp1 + qp1 ) - 1 ] ; } U [ qq ] ++ ; if
( colqp1 - 1 >= 0 ) { U [ 3 * colqp1 ] = 0.0 ; } } else { U [ 3 * colqp1 ] =
0.0 ; U [ 3 * colqp1 + 1 ] = 0.0 ; U [ 3 * colqp1 + 2 ] = 0.0 ; U [ qq ] =
1.0 ; } } for ( colqp1 = 2 ; colqp1 >= 0 ; colqp1 -- ) { if ( ( colqp1 + 1 <=
1 ) && ( e [ 0 ] != 0.0 ) ) { memcpy ( & A_p [ 0 ] , & V [ 0 ] , 9U * sizeof
( real_T ) ) ; dqzyu4u4dr ( 2 , - ( hxwoj44b1k ( 2 , V , 2 , V , 5 ) / V [ 1
] ) , 2 , A_p , 5 , V ) ; memcpy ( & A_p [ 0 ] , & V [ 0 ] , 9U * sizeof (
real_T ) ) ; dqzyu4u4dr ( 2 , - ( hxwoj44b1k ( 2 , V , 2 , V , 8 ) / V [ 1 ]
) , 2 , A_p , 8 , V ) ; } V [ 3 * colqp1 ] = 0.0 ; V [ 3 * colqp1 + 1 ] = 0.0
; V [ 3 * colqp1 + 2 ] = 0.0 ; V [ colqp1 + 3 * colqp1 ] = 1.0 ; } for ( qp1
= 0 ; qp1 < 3 ; qp1 ++ ) { smm1 = e [ qp1 ] ; if ( s_p [ qp1 ] != 0.0 ) { rt
= muDoubleScalarAbs ( s_p [ qp1 ] ) ; nrm = s_p [ qp1 ] / rt ; s_p [ qp1 ] =
rt ; if ( qp1 + 1 < 3 ) { smm1 /= nrm ; } qq = 3 * qp1 ; scalarLB = qq + 3 ;
vectorUB = qq + 1 ; for ( qjj = qq + 1 ; qjj <= vectorUB ; qjj += 2 ) { tmp =
_mm_loadu_pd ( & U [ qjj - 1 ] ) ; _mm_storeu_pd ( & U [ qjj - 1 ] ,
_mm_mul_pd ( tmp , _mm_set1_pd ( nrm ) ) ) ; } for ( qjj = scalarLB ; qjj <=
qq + 3 ; qjj ++ ) { U [ qjj - 1 ] *= nrm ; } } if ( ( qp1 + 1 < 3 ) && ( smm1
!= 0.0 ) ) { rt = muDoubleScalarAbs ( smm1 ) ; nrm = rt / smm1 ; smm1 = rt ;
s_p [ qp1 + 1 ] *= nrm ; colqp1 = ( qp1 + 1 ) * 3 ; scalarLB = colqp1 + 3 ;
vectorUB = colqp1 + 1 ; for ( qjj = colqp1 + 1 ; qjj <= vectorUB ; qjj += 2 )
{ tmp = _mm_loadu_pd ( & V [ qjj - 1 ] ) ; _mm_storeu_pd ( & V [ qjj - 1 ] ,
_mm_mul_pd ( tmp , _mm_set1_pd ( nrm ) ) ) ; } for ( qjj = scalarLB ; qjj <=
colqp1 + 3 ; qjj ++ ) { V [ qjj - 1 ] *= nrm ; } } e [ qp1 ] = smm1 ; } rt =
0.0 ; nrm = muDoubleScalarMax ( muDoubleScalarMax ( muDoubleScalarMax ( 0.0 ,
muDoubleScalarMax ( muDoubleScalarAbs ( s_p [ 0 ] ) , muDoubleScalarAbs ( e [
0 ] ) ) ) , muDoubleScalarMax ( muDoubleScalarAbs ( s_p [ 1 ] ) ,
muDoubleScalarAbs ( e [ 1 ] ) ) ) , muDoubleScalarMax ( muDoubleScalarAbs (
s_p [ 2 ] ) , muDoubleScalarAbs ( e [ 2 ] ) ) ) ; while ( ( m + 1 > 0 ) && (
! ( rt >= 75.0 ) ) ) { colqp1 = m ; qp1 = m ; exitg1 = false ; while ( ( !
exitg1 ) && ( qp1 > - 1 ) ) { colqp1 = qp1 ; if ( qp1 == 0 ) { exitg1 = true
; } else { ztest0 = muDoubleScalarAbs ( e [ qp1 - 1 ] ) ; if ( ( ztest0 <= (
muDoubleScalarAbs ( s_p [ qp1 - 1 ] ) + muDoubleScalarAbs ( s_p [ qp1 ] ) ) *
2.2204460492503131E-16 ) || ( ztest0 <= 1.0020841800044864E-292 ) || ( ( rt >
20.0 ) && ( ztest0 <= 2.2204460492503131E-16 * nrm ) ) ) { e [ qp1 - 1 ] =
0.0 ; exitg1 = true ; } else { qp1 -- ; } } } if ( colqp1 == m ) { ztest0 =
4.0 ; } else { qp1 = m + 1 ; qq = m + 1 ; exitg1 = false ; while ( ( ! exitg1
) && ( qq >= colqp1 ) ) { qp1 = qq ; if ( qq == colqp1 ) { exitg1 = true ; }
else { ztest0 = 0.0 ; if ( qq < m + 1 ) { ztest0 = muDoubleScalarAbs ( e [ qq
- 1 ] ) ; } if ( qq > colqp1 + 1 ) { ztest0 += muDoubleScalarAbs ( e [ qq - 2
] ) ; } ztest = muDoubleScalarAbs ( s_p [ qq - 1 ] ) ; if ( ( ztest <=
2.2204460492503131E-16 * ztest0 ) || ( ztest <= 1.0020841800044864E-292 ) ) {
s_p [ qq - 1 ] = 0.0 ; exitg1 = true ; } else { qq -- ; } } } if ( qp1 ==
colqp1 ) { ztest0 = 3.0 ; } else if ( m + 1 == qp1 ) { ztest0 = 1.0 ; } else
{ ztest0 = 2.0 ; colqp1 = qp1 ; } } switch ( ( int32_T ) ztest0 ) { case 1 :
ztest0 = e [ m - 1 ] ; e [ m - 1 ] = 0.0 ; for ( qq = m ; qq >= colqp1 + 1 ;
qq -- ) { smm1 = e [ 0 ] ; pwomuwmg0o ( s_p [ qq - 1 ] , ztest0 , & s_p [ qq
- 1 ] , & ztest0 , & ztest , & sqds ) ; if ( qq > colqp1 + 1 ) { ztest0 = -
sqds * e [ 0 ] ; smm1 = e [ 0 ] * ztest ; } memcpy ( & A_p [ 0 ] , & V [ 0 ]
, 9U * sizeof ( real_T ) ) ; jqns5sfonc ( A_p , ( qq - 1 ) * 3 + 1 , 3 * m +
1 , ztest , sqds , V ) ; e [ 0 ] = smm1 ; } break ; case 2 : ztest0 = e [
colqp1 - 1 ] ; e [ colqp1 - 1 ] = 0.0 ; for ( qp1 = colqp1 + 1 ; qp1 <= m + 1
; qp1 ++ ) { pwomuwmg0o ( s_p [ qp1 - 1 ] , ztest0 , & s_p [ qp1 - 1 ] , &
ztest , & sqds , & smm1 ) ; ztest0 = e [ qp1 - 1 ] * - smm1 ; e [ qp1 - 1 ]
*= sqds ; memcpy ( & A_p [ 0 ] , & U [ 0 ] , 9U * sizeof ( real_T ) ) ;
jqns5sfonc ( A_p , ( qp1 - 1 ) * 3 + 1 , ( colqp1 - 1 ) * 3 + 1 , sqds , smm1
, U ) ; } break ; case 3 : ztest = muDoubleScalarMax ( muDoubleScalarMax (
muDoubleScalarMax ( muDoubleScalarMax ( muDoubleScalarAbs ( s_p [ m ] ) ,
muDoubleScalarAbs ( s_p [ m - 1 ] ) ) , muDoubleScalarAbs ( e [ m - 1 ] ) ) ,
muDoubleScalarAbs ( s_p [ colqp1 ] ) ) , muDoubleScalarAbs ( e [ colqp1 ] ) )
; ztest0 = s_p [ m ] / ztest ; smm1 = s_p [ m - 1 ] / ztest ; emm1 = e [ m -
1 ] / ztest ; sqds = s_p [ colqp1 ] / ztest ; smm1 = ( ( smm1 + ztest0 ) * (
smm1 - ztest0 ) + emm1 * emm1 ) / 2.0 ; emm1 *= ztest0 ; emm1 *= emm1 ; if (
( smm1 != 0.0 ) || ( emm1 != 0.0 ) ) { shift = muDoubleScalarSqrt ( smm1 *
smm1 + emm1 ) ; if ( smm1 < 0.0 ) { shift = - shift ; } shift = emm1 / ( smm1
+ shift ) ; } else { shift = 0.0 ; } ztest0 = ( sqds + ztest0 ) * ( sqds -
ztest0 ) + shift ; ztest = e [ colqp1 ] / ztest * sqds ; for ( qq = colqp1 +
1 ; qq <= m ; qq ++ ) { pwomuwmg0o ( ztest0 , ztest , & sqds , & smm1 , &
emm1 , & shift ) ; if ( qq > colqp1 + 1 ) { e [ 0 ] = sqds ; } ztest0 = s_p [
qq - 1 ] * emm1 + e [ qq - 1 ] * shift ; e [ qq - 1 ] = e [ qq - 1 ] * emm1 -
s_p [ qq - 1 ] * shift ; ztest = shift * s_p [ qq ] ; s_p [ qq ] *= emm1 ;
memcpy ( & A_p [ 0 ] , & V [ 0 ] , 9U * sizeof ( real_T ) ) ; jqns5sfonc (
A_p , ( qq - 1 ) * 3 + 1 , 3 * qq + 1 , emm1 , shift , V ) ; pwomuwmg0o (
ztest0 , ztest , & s_p [ qq - 1 ] , & sqds , & smm1 , & emm1 ) ; ztest0 = e [
qq - 1 ] * smm1 + emm1 * s_p [ qq ] ; s_p [ qq ] = e [ qq - 1 ] * - emm1 +
smm1 * s_p [ qq ] ; ztest = emm1 * e [ qq ] ; e [ qq ] *= smm1 ; memcpy ( &
A_p [ 0 ] , & U [ 0 ] , 9U * sizeof ( real_T ) ) ; jqns5sfonc ( A_p , ( qq -
1 ) * 3 + 1 , 3 * qq + 1 , smm1 , emm1 , U ) ; } e [ m - 1 ] = ztest0 ; rt ++
; break ; default : if ( s_p [ colqp1 ] < 0.0 ) { s_p [ colqp1 ] = - s_p [
colqp1 ] ; qq = 3 * colqp1 ; scalarLB = qq + 3 ; vectorUB = qq + 1 ; for (
qjj = qq + 1 ; qjj <= vectorUB ; qjj += 2 ) { tmp = _mm_loadu_pd ( & V [ qjj
- 1 ] ) ; _mm_storeu_pd ( & V [ qjj - 1 ] , _mm_mul_pd ( tmp , _mm_set1_pd (
- 1.0 ) ) ) ; } for ( qjj = scalarLB ; qjj <= qq + 3 ; qjj ++ ) { V [ qjj - 1
] = - V [ qjj - 1 ] ; } } qp1 = colqp1 + 1 ; while ( ( colqp1 + 1 < 3 ) && (
s_p [ colqp1 ] < s_p [ qp1 ] ) ) { rt = s_p [ colqp1 ] ; s_p [ colqp1 ] = s_p
[ qp1 ] ; s_p [ qp1 ] = rt ; memcpy ( & A_p [ 0 ] , & V [ 0 ] , 9U * sizeof (
real_T ) ) ; h1cxhqfs1e ( A_p , 3 * colqp1 + 1 , ( colqp1 + 1 ) * 3 + 1 , V )
; memcpy ( & A_p [ 0 ] , & U [ 0 ] , 9U * sizeof ( real_T ) ) ; h1cxhqfs1e (
A_p , 3 * colqp1 + 1 , ( colqp1 + 1 ) * 3 + 1 , U ) ; colqp1 = qp1 ; qp1 ++ ;
} rt = 0.0 ; m -- ; break ; } } s [ 0 ] = s_p [ 0 ] ; s [ 1 ] = s_p [ 1 ] ; s
[ 2 ] = s_p [ 2 ] ; } static void pbvdpfzges ( const real_T x [ 6 ] ,
lnin0tndd2 * args , real_T * cost , real_T W [ 36 ] , klgljlxbmf * Jac ,
lnin0tndd2 * * b_args ) { __m128d tmp_p ; e1n154mvbh * bodyName ; klgljlxbmf
* J ; klgljlxbmf * y_p ; m0caqfktx0 * treeInternal ; creal_T tmp ; creal_T u
; creal_T u_p ; creal_T v_p ; real_T T_data [ 16 ] ; real_T Td [ 16 ] ;
real_T T [ 9 ] ; real_T V [ 9 ] ; real_T b_U [ 9 ] ; real_T y_e [ 9 ] ;
real_T e [ 6 ] ; real_T y [ 6 ] ; real_T b_s [ 3 ] ; real_T v [ 3 ] ; real_T
vspecial_data [ 3 ] ; real_T q ; real_T t4 ; int32_T T_size [ 2 ] ; int32_T
boffset ; int32_T iy ; int32_T loop_ub ; int32_T n ; boolean_T exitg1 ;
boolean_T xneg ; * b_args = args ; treeInternal = args -> Robot ; m2n1niwoq4
( & bodyName , 2 ) ; loop_ub = bodyName -> size [ 0 ] * bodyName -> size [ 1
] ; bodyName -> size [ 0 ] = 1 ; bodyName -> size [ 1 ] = args -> BodyName ->
size [ 1 ] ; e3n3mnwk4b ( bodyName , loop_ub ) ; loop_ub = args -> BodyName
-> size [ 1 ] ; for ( iy = 0 ; iy < loop_ub ; iy ++ ) { bodyName -> data [ iy
] = args -> BodyName -> data [ iy ] ; } for ( iy = 0 ; iy < 16 ; iy ++ ) { Td
[ iy ] = args -> Tform [ iy ] ; } for ( iy = 0 ; iy < 36 ; iy ++ ) { W [ iy ]
= args -> WeightMatrix [ iy ] ; } calykgtvmm ( & J , 2 ) ; p3z13glynv (
treeInternal , x , bodyName , T_data , T_size , J ) ; fagcrr3avn ( & bodyName
) ; loop_ub = Jac -> size [ 0 ] * Jac -> size [ 1 ] ; Jac -> size [ 0 ] = 6 ;
Jac -> size [ 1 ] = J -> size [ 1 ] ; miyl1rq4uj ( Jac , loop_ub ) ; loop_ub
= 6 * J -> size [ 1 ] ; boffset = ( loop_ub / 2 ) << 1 ; n = boffset - 2 ;
for ( iy = 0 ; iy <= n ; iy += 2 ) { tmp_p = _mm_loadu_pd ( & J -> data [ iy
] ) ; _mm_storeu_pd ( & Jac -> data [ iy ] , _mm_mul_pd ( tmp_p , _mm_set1_pd
( - 1.0 ) ) ) ; } for ( iy = boffset ; iy < loop_ub ; iy ++ ) { Jac -> data [
iy ] = - J -> data [ iy ] ; } bdhoypqe5x ( & J ) ; for ( iy = 0 ; iy < 3 ; iy
++ ) { T [ 3 * iy ] = T_data [ iy ] ; T [ 3 * iy + 1 ] = T_data [ iy + T_size
[ 0 ] ] ; T [ 3 * iy + 2 ] = T_data [ ( T_size [ 0 ] << 1 ) + iy ] ; for (
loop_ub = 0 ; loop_ub < 3 ; loop_ub ++ ) { y_e [ loop_ub + 3 * iy ] = 0.0 ;
y_e [ loop_ub + 3 * iy ] += T [ 3 * iy ] * Td [ loop_ub ] ; y_e [ loop_ub + 3
* iy ] += T [ 3 * iy + 1 ] * Td [ loop_ub + 4 ] ; y_e [ loop_ub + 3 * iy ] +=
T [ 3 * iy + 2 ] * Td [ loop_ub + 8 ] ; } } u . re = ( ( ( y_e [ 0 ] + y_e [
4 ] ) + y_e [ 8 ] ) - 1.0 ) * 0.5 ; if ( ! ( muDoubleScalarAbs ( u . re ) >
1.0 ) ) { v_p . re = muDoubleScalarAcos ( u . re ) ; } else { u_p . re = u .
re + 1.0 ; u_p . im = 0.0 ; tmp . re = 1.0 - u . re ; tmp . im = 0.0 ; v_p .
re = 2.0 * muDoubleScalarAtan2 ( ( hv02b2lq4w ( tmp ) ) . re , ( hv02b2lq4w (
u_p ) ) . re ) ; } t4 = 2.0 * muDoubleScalarSin ( v_p . re ) ; v [ 0 ] = (
y_e [ 5 ] - y_e [ 7 ] ) / t4 ; v [ 1 ] = ( y_e [ 6 ] - y_e [ 2 ] ) / t4 ; v [
2 ] = ( y_e [ 1 ] - y_e [ 3 ] ) / t4 ; if ( muDoubleScalarIsNaN ( v_p . re )
|| muDoubleScalarIsInf ( v_p . re ) ) { t4 = ( rtNaN ) ; } else if ( v_p . re
== 0.0 ) { t4 = 0.0 ; } else { t4 = muDoubleScalarRem ( v_p . re ,
3.1415926535897931 ) ; xneg = ( t4 == 0.0 ) ; if ( ! xneg ) { q =
muDoubleScalarAbs ( v_p . re / 3.1415926535897931 ) ; xneg = ! (
muDoubleScalarAbs ( q - muDoubleScalarFloor ( q + 0.5 ) ) >
2.2204460492503131E-16 * q ) ; } if ( xneg ) { t4 = 0.0 ; } else if ( v_p .
re < 0.0 ) { t4 += 3.1415926535897931 ; } } xneg = true ; iy = 0 ; exitg1 =
false ; while ( ( ! exitg1 ) && ( iy < 3 ) ) { if ( ! ( v [ iy ] == 0.0 ) ) {
xneg = false ; exitg1 = true ; } else { iy ++ ; } } xneg = ( ( t4 == 0.0 ) ||
xneg ) ; if ( xneg ) { for ( iy = 0 ; iy < 3 ; iy ++ ) { vspecial_data [ iy ]
= 0.0 ; } for ( loop_ub = 0 ; loop_ub < 1 ; loop_ub ++ ) { memset ( & T [ 0 ]
, 0 , 9U * sizeof ( real_T ) ) ; T [ 0 ] = 1.0 ; T [ 4 ] = 1.0 ; T [ 8 ] =
1.0 ; for ( iy = 0 ; iy < 9 ; iy ++ ) { T [ iy ] -= y_e [ iy ] ; } xneg =
true ; for ( iy = 0 ; iy < 9 ; iy ++ ) { if ( xneg ) { t4 = T [ iy ] ; if ( (
! muDoubleScalarIsInf ( t4 ) ) && ( ! muDoubleScalarIsNaN ( t4 ) ) ) { } else
{ xneg = false ; } } else { xneg = false ; } } if ( xneg ) { o5vuefzdbw ( T ,
b_U , b_s , V ) ; } else { for ( iy = 0 ; iy < 9 ; iy ++ ) { V [ iy ] = (
rtNaN ) ; } } vspecial_data [ 0 ] = V [ 6 ] ; vspecial_data [ 1 ] = V [ 7 ] ;
vspecial_data [ 2 ] = V [ 8 ] ; } loop_ub = 0 ; for ( iy = 0 ; iy < 1 ; iy ++
) { loop_ub ++ ; } if ( loop_ub - 1 >= 0 ) { v [ 0 ] = vspecial_data [ 0 ] ;
v [ 1 ] = vspecial_data [ 1 ] ; v [ 2 ] = vspecial_data [ 2 ] ; } } t4 = 1.0
/ muDoubleScalarSqrt ( ( v [ 0 ] * v [ 0 ] + v [ 1 ] * v [ 1 ] ) + v [ 2 ] *
v [ 2 ] ) ; v [ 0 ] *= t4 ; v [ 1 ] *= t4 ; e [ 0 ] = v_p . re * v [ 0 ] ; e
[ 3 ] = Td [ 12 ] - T_data [ T_size [ 0 ] * 3 ] ; e [ 1 ] = v_p . re * v [ 1
] ; e [ 4 ] = Td [ 13 ] - T_data [ T_size [ 0 ] * 3 + 1 ] ; e [ 2 ] = v [ 2 ]
* t4 * v_p . re ; e [ 5 ] = Td [ 14 ] - T_data [ T_size [ 0 ] * 3 + 2 ] ;
loop_ub = args -> ErrTemp -> size [ 0 ] ; args -> ErrTemp -> size [ 0 ] = 6 ;
miyl1rq4uj ( args -> ErrTemp , loop_ub ) ; for ( iy = 0 ; iy < 6 ; iy ++ ) {
args -> ErrTemp -> data [ iy ] = e [ iy ] ; } t4 = 0.0 ; for ( iy = 0 ; iy <
6 ; iy ++ ) { y [ iy ] = 0.0 ; for ( loop_ub = 0 ; loop_ub < 6 ; loop_ub ++ )
{ y [ iy ] += W [ 6 * iy + loop_ub ] * ( 0.5 * e [ loop_ub ] ) ; } t4 += y [
iy ] * e [ iy ] ; } args -> CostTemp = t4 ; for ( iy = 0 ; iy < 6 ; iy ++ ) {
y [ iy ] = 0.0 ; for ( loop_ub = 0 ; loop_ub < 6 ; loop_ub ++ ) { y [ iy ] +=
W [ 6 * iy + loop_ub ] * e [ loop_ub ] ; } } n = Jac -> size [ 1 ] - 1 ;
calykgtvmm ( & y_p , 2 ) ; loop_ub = y_p -> size [ 0 ] * y_p -> size [ 1 ] ;
y_p -> size [ 0 ] = 1 ; y_p -> size [ 1 ] = Jac -> size [ 1 ] ; miyl1rq4uj (
y_p , loop_ub ) ; for ( loop_ub = 0 ; loop_ub <= n ; loop_ub ++ ) { boffset =
loop_ub * 6 - 1 ; t4 = 0.0 ; for ( iy = 0 ; iy < 6 ; iy ++ ) { t4 += Jac ->
data [ ( boffset + iy ) + 1 ] * y [ iy ] ; } y_p -> data [ loop_ub ] = t4 ; }
loop_ub = args -> GradTemp -> size [ 0 ] ; args -> GradTemp -> size [ 0 ] =
y_p -> size [ 1 ] ; miyl1rq4uj ( args -> GradTemp , loop_ub ) ; loop_ub = y_p
-> size [ 1 ] ; for ( iy = 0 ; iy < loop_ub ; iy ++ ) { args -> GradTemp ->
data [ iy ] = y_p -> data [ iy ] ; } bdhoypqe5x ( & y_p ) ; * cost = args ->
CostTemp ; } static void l45e0kru51 ( m5kgub1d5k * * pEmxArray , int32_T
numDimensions ) { m5kgub1d5k * emxArray ; int32_T i ; * pEmxArray = (
m5kgub1d5k * ) malloc ( sizeof ( m5kgub1d5k ) ) ; emxArray = * pEmxArray ;
emxArray -> data = ( boolean_T * ) NULL ; emxArray -> numDimensions =
numDimensions ; emxArray -> size = ( int32_T * ) malloc ( sizeof ( int32_T )
* ( uint32_T ) numDimensions ) ; emxArray -> allocatedSize = 0 ; emxArray ->
canFreeData = true ; for ( i = 0 ; i < numDimensions ; i ++ ) { emxArray ->
size [ i ] = 0 ; } } static void ks3ealo3sd ( ncvluzgkod * * pEmxArray ,
int32_T numDimensions ) { ncvluzgkod * emxArray ; int32_T i ; * pEmxArray = (
ncvluzgkod * ) malloc ( sizeof ( ncvluzgkod ) ) ; emxArray = * pEmxArray ;
emxArray -> data = ( int32_T * ) NULL ; emxArray -> numDimensions =
numDimensions ; emxArray -> size = ( int32_T * ) malloc ( sizeof ( int32_T )
* ( uint32_T ) numDimensions ) ; emxArray -> allocatedSize = 0 ; emxArray ->
canFreeData = true ; for ( i = 0 ; i < numDimensions ; i ++ ) { emxArray ->
size [ i ] = 0 ; } } static void dw1xs30knw ( m5kgub1d5k * emxArray , int32_T
oldNumel ) { int32_T i ; int32_T newNumel ; void * newData ; if ( oldNumel <
0 ) { oldNumel = 0 ; } newNumel = 1 ; for ( i = 0 ; i < emxArray ->
numDimensions ; i ++ ) { newNumel *= emxArray -> size [ i ] ; } if ( newNumel
> emxArray -> allocatedSize ) { i = emxArray -> allocatedSize ; if ( i < 16 )
{ i = 16 ; } while ( i < newNumel ) { if ( i > 1073741823 ) { i = MAX_int32_T
; } else { i <<= 1 ; } } newData = calloc ( ( uint32_T ) i , sizeof (
boolean_T ) ) ; if ( emxArray -> data != NULL ) { memcpy ( newData , emxArray
-> data , sizeof ( boolean_T ) * ( uint32_T ) oldNumel ) ; if ( emxArray ->
canFreeData ) { free ( emxArray -> data ) ; } } emxArray -> data = (
boolean_T * ) newData ; emxArray -> allocatedSize = i ; emxArray ->
canFreeData = true ; } } static void l5ks21uytpa ( const klgljlxbmf * A ,
const real_T B_e [ 6 ] , klgljlxbmf * C ) { int32_T b_i ; int32_T c_i ;
int32_T inner ; int32_T m ; m = A -> size [ 1 ] - 1 ; inner = A -> size [ 0 ]
- 1 ; b_i = C -> size [ 0 ] ; C -> size [ 0 ] = A -> size [ 1 ] ; miyl1rq4uj
( C , b_i ) ; if ( m >= 0 ) { memset ( & C -> data [ 0 ] , 0 , ( uint32_T ) (
m + 1 ) * sizeof ( real_T ) ) ; } for ( b_i = 0 ; b_i <= inner ; b_i ++ ) {
for ( c_i = 0 ; c_i <= m ; c_i ++ ) { C -> data [ c_i ] += A -> data [ c_i *
A -> size [ 0 ] + b_i ] * B_e [ b_i ] ; } } } static void offa2br1khoegk (
m5kgub1d5k * in1 , const klgljlxbmf * in2 , const ecfe5we0wc * in3 ) {
int32_T i ; int32_T loop_ub ; int32_T stride_0_0 ; int32_T stride_1_0 ; i =
in1 -> size [ 0 ] ; in1 -> size [ 0 ] = in3 -> ConstraintBound -> size [ 0 ]
== 1 ? in2 -> size [ 0 ] : in3 -> ConstraintBound -> size [ 0 ] ; dw1xs30knw
( in1 , i ) ; stride_0_0 = ( in2 -> size [ 0 ] != 1 ) ; stride_1_0 = ( in3 ->
ConstraintBound -> size [ 0 ] != 1 ) ; loop_ub = in3 -> ConstraintBound ->
size [ 0 ] == 1 ? in2 -> size [ 0 ] : in3 -> ConstraintBound -> size [ 0 ] ;
for ( i = 0 ; i < loop_ub ; i ++ ) { in1 -> data [ i ] = ( in2 -> data [ i *
stride_0_0 ] >= in3 -> ConstraintBound -> data [ i * stride_1_0 ] ) ; } }
static void os5bv5kg0d ( ncvluzgkod * emxArray , int32_T oldNumel ) { int32_T
i ; int32_T newNumel ; void * newData ; if ( oldNumel < 0 ) { oldNumel = 0 ;
} newNumel = 1 ; for ( i = 0 ; i < emxArray -> numDimensions ; i ++ ) {
newNumel *= emxArray -> size [ i ] ; } if ( newNumel > emxArray ->
allocatedSize ) { i = emxArray -> allocatedSize ; if ( i < 16 ) { i = 16 ; }
while ( i < newNumel ) { if ( i > 1073741823 ) { i = MAX_int32_T ; } else { i
<<= 1 ; } } newData = calloc ( ( uint32_T ) i , sizeof ( int32_T ) ) ; if (
emxArray -> data != NULL ) { memcpy ( newData , emxArray -> data , sizeof (
int32_T ) * ( uint32_T ) oldNumel ) ; if ( emxArray -> canFreeData ) { free (
emxArray -> data ) ; } } emxArray -> data = ( int32_T * ) newData ; emxArray
-> allocatedSize = i ; emxArray -> canFreeData = true ; } } static void
l5ks21uytpaq ( const real_T A [ 36 ] , const klgljlxbmf * B_i , klgljlxbmf *
C ) { real_T s ; int32_T b_i ; int32_T b_j ; int32_T b_k ; int32_T boffset ;
int32_T coffset ; int32_T n ; n = B_i -> size [ 1 ] - 1 ; b_j = C -> size [ 0
] * C -> size [ 1 ] ; C -> size [ 0 ] = 6 ; C -> size [ 1 ] = B_i -> size [ 1
] ; miyl1rq4uj ( C , b_j ) ; for ( b_j = 0 ; b_j <= n ; b_j ++ ) { coffset =
b_j * 6 - 1 ; boffset = b_j * B_i -> size [ 0 ] - 1 ; for ( b_i = 0 ; b_i < 6
; b_i ++ ) { s = 0.0 ; for ( b_k = 0 ; b_k < 6 ; b_k ++ ) { s += A [ b_k * 6
+ b_i ] * B_i -> data [ ( boffset + b_k ) + 1 ] ; } C -> data [ ( coffset +
b_i ) + 1 ] = s ; } } } static real_T fdzsuttcyn1 ( const real_T x [ 6 ] ) {
real_T absxk ; real_T scale ; real_T t ; real_T y ; int32_T b_k ; y = 0.0 ;
scale = 3.3121686421112381E-170 ; for ( b_k = 0 ; b_k < 6 ; b_k ++ ) { absxk
= muDoubleScalarAbs ( x [ b_k ] ) ; if ( absxk > scale ) { t = scale / absxk
; y = y * t * t + 1.0 ; scale = absxk ; } else { t = absxk / scale ; y += t *
t ; } } return scale * muDoubleScalarSqrt ( y ) ; } static real_T ethycgs32k
( real_T tstart_tv_sec , real_T tstart_tv_nsec ) { coderTimespec b_timespec ;
if ( ! rtDW . gcwd0zaucf ) { rtDW . gcwd0zaucf = true ;
coderInitTimeFunctions ( & rtDW . ccbb0ktavq ) ; }
coderTimeClockGettimeMonotonic ( & b_timespec , rtDW . ccbb0ktavq ) ; return
( b_timespec . tv_nsec - tstart_tv_nsec ) / 1.0E+9 + ( b_timespec . tv_sec -
tstart_tv_sec ) ; } static void l5ks21uytpaq2 ( const klgljlxbmf * A , const
klgljlxbmf * B_m , klgljlxbmf * C ) { real_T bkj ; int32_T b_i ; int32_T b_j
; int32_T boffset ; int32_T c_i ; int32_T coffset ; int32_T inner ; int32_T m
; int32_T n ; m = A -> size [ 1 ] - 1 ; inner = A -> size [ 0 ] - 1 ; n = B_m
-> size [ 1 ] - 1 ; b_j = C -> size [ 0 ] * C -> size [ 1 ] ; C -> size [ 0 ]
= A -> size [ 1 ] ; C -> size [ 1 ] = B_m -> size [ 1 ] ; miyl1rq4uj ( C ,
b_j ) ; for ( b_j = 0 ; b_j <= n ; b_j ++ ) { coffset = ( m + 1 ) * b_j - 1 ;
boffset = b_j * B_m -> size [ 0 ] - 1 ; if ( m >= 0 ) { memset ( & C -> data
[ coffset + 1 ] , 0 , ( uint32_T ) ( ( ( m + coffset ) - coffset ) + 1 ) *
sizeof ( real_T ) ) ; } for ( b_i = 0 ; b_i <= inner ; b_i ++ ) { bkj = B_m
-> data [ ( boffset + b_i ) + 1 ] ; for ( c_i = 0 ; c_i <= m ; c_i ++ ) { C
-> data [ ( coffset + c_i ) + 1 ] += A -> data [ c_i * A -> size [ 0 ] + b_i
] * bkj ; } } } } static real_T iq4v5bez02ka ( int32_T n , const klgljlxbmf *
x , int32_T ix0 ) { real_T absxk ; real_T scale ; real_T t ; real_T y ;
int32_T k ; int32_T kend ; y = 0.0 ; if ( n < 1 ) { } else if ( n == 1 ) { y
= muDoubleScalarAbs ( x -> data [ ix0 - 1 ] ) ; } else { scale =
3.3121686421112381E-170 ; kend = ix0 + n ; for ( k = ix0 ; k < kend ; k ++ )
{ absxk = muDoubleScalarAbs ( x -> data [ k - 1 ] ) ; if ( absxk > scale ) {
t = scale / absxk ; y = y * t * t + 1.0 ; scale = absxk ; } else { t = absxk
/ scale ; y += t * t ; } } y = scale * muDoubleScalarSqrt ( y ) ; } return y
; } static void preyilpnl4 ( ncvluzgkod * * pEmxArray ) { if ( * pEmxArray !=
( ncvluzgkod * ) NULL ) { if ( ( ( * pEmxArray ) -> data != ( int32_T * )
NULL ) && ( * pEmxArray ) -> canFreeData ) { free ( ( * pEmxArray ) -> data )
; } free ( ( * pEmxArray ) -> size ) ; free ( * pEmxArray ) ; * pEmxArray = (
ncvluzgkod * ) NULL ; } } static void ldurgmcftq ( const klgljlxbmf * A ,
klgljlxbmf * b_A , klgljlxbmf * tau , ncvluzgkod * jpvt ) { klgljlxbmf * vn1
; klgljlxbmf * vn2 ; klgljlxbmf * work ; ncvluzgkod * b_jpvt ; ncvluzgkod *
b_jpvt_p ; real_T absxk ; real_T scale ; real_T smax ; real_T t ; int32_T d ;
int32_T ix ; int32_T iy ; int32_T jy ; int32_T kend ; int32_T knt ; int32_T
lastv ; int32_T loop_ub ; int32_T ma ; int32_T ma_p ; int32_T mmi ; int32_T
na ; int32_T na_p ; int32_T nmi ; int32_T pvt ; static const int32_T offsets
[ 4 ] = { 0 , 1 , 2 , 3 } ; __m128d tmp ; static const int32_T offsets_p [ 4
] = { 0 , 1 , 2 , 3 } ; int32_T exitg1 ; boolean_T exitg2 ; ma = A -> size [
0 ] ; na = A -> size [ 1 ] ; loop_ub = b_A -> size [ 0 ] * b_A -> size [ 1 ]
; b_A -> size [ 0 ] = A -> size [ 0 ] ; b_A -> size [ 1 ] = A -> size [ 1 ] ;
miyl1rq4uj ( b_A , loop_ub ) ; loop_ub = A -> size [ 0 ] * A -> size [ 1 ] ;
if ( loop_ub - 1 >= 0 ) { memcpy ( & b_A -> data [ 0 ] , & A -> data [ 0 ] ,
( uint32_T ) loop_ub * sizeof ( real_T ) ) ; } ma_p = A -> size [ 0 ] ; na_p
= A -> size [ 1 ] ; ma_p = muIntScalarMin_sint32 ( ma_p , na_p ) ; loop_ub =
tau -> size [ 0 ] ; tau -> size [ 0 ] = ma_p ; miyl1rq4uj ( tau , loop_ub ) ;
if ( ma_p - 1 >= 0 ) { memset ( & tau -> data [ 0 ] , 0 , ( uint32_T ) ma_p *
sizeof ( real_T ) ) ; } ks3ealo3sd ( & b_jpvt , 2 ) ; ks3ealo3sd ( & b_jpvt_p
, 2 ) ; calykgtvmm ( & work , 1 ) ; calykgtvmm ( & vn1 , 1 ) ; calykgtvmm ( &
vn2 , 1 ) ; if ( ( A -> size [ 0 ] == 0 ) || ( A -> size [ 1 ] == 0 ) || (
muIntScalarMin_sint32 ( ma , na ) < 1 ) ) { loop_ub = b_jpvt -> size [ 0 ] *
b_jpvt -> size [ 1 ] ; b_jpvt -> size [ 0 ] = 1 ; b_jpvt -> size [ 1 ] = A ->
size [ 1 ] ; os5bv5kg0d ( b_jpvt , loop_ub ) ; loop_ub = A -> size [ 1 ] ; if
( loop_ub - 1 >= 0 ) { memset ( & b_jpvt -> data [ 0 ] , 0 , ( uint32_T )
loop_ub * sizeof ( int32_T ) ) ; } iy = ( A -> size [ 1 ] / 4 ) << 2 ; jy =
iy - 4 ; for ( loop_ub = 0 ; loop_ub <= jy ; loop_ub += 4 ) {
_mm_storeu_si128 ( ( __m128i * ) & b_jpvt -> data [ loop_ub ] , _mm_add_epi32
( _mm_add_epi32 ( _mm_set1_epi32 ( loop_ub ) , _mm_loadu_si128 ( ( const
__m128i * ) & offsets [ 0 ] ) ) , _mm_set1_epi32 ( 1 ) ) ) ; } for ( loop_ub
= iy ; loop_ub < na ; loop_ub ++ ) { b_jpvt -> data [ loop_ub ] = loop_ub + 1
; } } else { loop_ub = b_jpvt -> size [ 0 ] * b_jpvt -> size [ 1 ] ; b_jpvt
-> size [ 0 ] = 1 ; b_jpvt -> size [ 1 ] = A -> size [ 1 ] ; os5bv5kg0d (
b_jpvt , loop_ub ) ; loop_ub = A -> size [ 1 ] ; memset ( & b_jpvt -> data [
0 ] , 0 , ( uint32_T ) loop_ub * sizeof ( int32_T ) ) ; iy = ( A -> size [ 1
] / 4 ) << 2 ; jy = iy - 4 ; for ( na_p = 0 ; na_p <= jy ; na_p += 4 ) {
_mm_storeu_si128 ( ( __m128i * ) & b_jpvt -> data [ na_p ] , _mm_add_epi32 (
_mm_add_epi32 ( _mm_set1_epi32 ( na_p ) , _mm_loadu_si128 ( ( const __m128i *
) & offsets_p [ 0 ] ) ) , _mm_set1_epi32 ( 1 ) ) ) ; } for ( na_p = iy ; na_p
< na ; na_p ++ ) { b_jpvt -> data [ na_p ] = na_p + 1 ; } loop_ub = b_jpvt_p
-> size [ 0 ] * b_jpvt_p -> size [ 1 ] ; b_jpvt_p -> size [ 0 ] = 1 ;
b_jpvt_p -> size [ 1 ] = b_jpvt -> size [ 1 ] ; os5bv5kg0d ( b_jpvt_p ,
loop_ub ) ; loop_ub = b_jpvt -> size [ 1 ] ; memcpy ( & b_jpvt_p -> data [ 0
] , & b_jpvt -> data [ 0 ] , ( uint32_T ) loop_ub * sizeof ( int32_T ) ) ;
loop_ub = b_A -> size [ 0 ] * b_A -> size [ 1 ] ; b_A -> size [ 0 ] = A ->
size [ 0 ] ; b_A -> size [ 1 ] = A -> size [ 1 ] ; miyl1rq4uj ( b_A , loop_ub
) ; loop_ub = A -> size [ 0 ] * A -> size [ 1 ] ; if ( loop_ub - 1 >= 0 ) {
memcpy ( & b_A -> data [ 0 ] , & A -> data [ 0 ] , ( uint32_T ) loop_ub *
sizeof ( real_T ) ) ; } ma_p = A -> size [ 0 ] ; na_p = muIntScalarMin_sint32
( ma , na ) - 1 ; loop_ub = work -> size [ 0 ] ; work -> size [ 0 ] = A ->
size [ 1 ] ; miyl1rq4uj ( work , loop_ub ) ; loop_ub = A -> size [ 1 ] ;
memset ( & work -> data [ 0 ] , 0 , ( uint32_T ) loop_ub * sizeof ( real_T )
) ; loop_ub = vn1 -> size [ 0 ] ; vn1 -> size [ 0 ] = A -> size [ 1 ] ;
miyl1rq4uj ( vn1 , loop_ub ) ; loop_ub = A -> size [ 1 ] ; memset ( & vn1 ->
data [ 0 ] , 0 , ( uint32_T ) loop_ub * sizeof ( real_T ) ) ; loop_ub = vn2
-> size [ 0 ] ; vn2 -> size [ 0 ] = A -> size [ 1 ] ; miyl1rq4uj ( vn2 ,
loop_ub ) ; loop_ub = A -> size [ 1 ] ; memset ( & vn2 -> data [ 0 ] , 0 , (
uint32_T ) loop_ub * sizeof ( real_T ) ) ; for ( loop_ub = 0 ; loop_ub < na ;
loop_ub ++ ) { ix = loop_ub * ma_p ; smax = 0.0 ; if ( ma < 1 ) { } else if (
ma == 1 ) { smax = muDoubleScalarAbs ( A -> data [ ix ] ) ; } else { scale =
3.3121686421112381E-170 ; kend = ix + ma ; for ( lastv = ix + 1 ; lastv <=
kend ; lastv ++ ) { absxk = muDoubleScalarAbs ( A -> data [ lastv - 1 ] ) ;
if ( absxk > scale ) { t = scale / absxk ; smax = smax * t * t + 1.0 ; scale
= absxk ; } else { t = absxk / scale ; smax += t * t ; } } smax = scale *
muDoubleScalarSqrt ( smax ) ; } vn2 -> data [ loop_ub ] = smax ; vn1 -> data
[ loop_ub ] = smax ; } for ( loop_ub = 0 ; loop_ub <= na_p ; loop_ub ++ ) {
kend = loop_ub * ma_p + loop_ub ; nmi = ( na - loop_ub ) - 2 ; mmi = ( ma -
loop_ub ) - 1 ; if ( nmi + 2 < 1 ) { pvt = 0 ; } else { pvt = 1 ; if ( nmi +
2 > 1 ) { smax = vn1 -> data [ loop_ub ] ; for ( lastv = 2 ; lastv <= nmi + 2
; lastv ++ ) { scale = vn1 -> data [ ( loop_ub + lastv ) - 1 ] ; if ( scale >
smax ) { pvt = lastv ; smax = scale ; } } } } pvt = ( loop_ub + pvt ) - 1 ;
if ( pvt + 1 != loop_ub + 1 ) { ix = pvt * ma_p ; iy = loop_ub * ma_p ; for (
lastv = 0 ; lastv < ma ; lastv ++ ) { scale = b_A -> data [ ix + lastv ] ;
b_A -> data [ ix + lastv ] = b_A -> data [ iy + lastv ] ; b_A -> data [ iy +
lastv ] = scale ; } ix = b_jpvt_p -> data [ pvt ] ; b_jpvt_p -> data [ pvt ]
= b_jpvt_p -> data [ loop_ub ] ; b_jpvt_p -> data [ loop_ub ] = ix ; vn1 ->
data [ pvt ] = vn1 -> data [ loop_ub ] ; vn2 -> data [ pvt ] = vn2 -> data [
loop_ub ] ; } if ( loop_ub + 1 < ma ) { ix = kend + 2 ; smax = b_A -> data [
kend ] ; tau -> data [ loop_ub ] = 0.0 ; if ( mmi + 1 > 0 ) { scale =
iq4v5bez02ka ( mmi , b_A , kend + 2 ) ; if ( scale != 0.0 ) { scale =
muDoubleScalarHypot ( b_A -> data [ kend ] , scale ) ; if ( b_A -> data [
kend ] >= 0.0 ) { scale = - scale ; } if ( muDoubleScalarAbs ( scale ) <
1.0020841800044864E-292 ) { knt = - 1 ; do { knt ++ ; pvt = ( kend + mmi ) +
1 ; iy = ( ( ( ( ( pvt - kend ) - 1 ) / 2 ) << 1 ) + kend ) + 2 ; jy = iy - 2
; for ( lastv = ix ; lastv <= jy ; lastv += 2 ) { tmp = _mm_loadu_pd ( & b_A
-> data [ lastv - 1 ] ) ; _mm_storeu_pd ( & b_A -> data [ lastv - 1 ] ,
_mm_mul_pd ( tmp , _mm_set1_pd ( 9.9792015476736E+291 ) ) ) ; } for ( lastv =
iy ; lastv <= pvt ; lastv ++ ) { b_A -> data [ lastv - 1 ] *=
9.9792015476736E+291 ; } scale *= 9.9792015476736E+291 ; smax *=
9.9792015476736E+291 ; } while ( ( muDoubleScalarAbs ( scale ) <
1.0020841800044864E-292 ) && ( knt + 1 < 20 ) ) ; scale = muDoubleScalarHypot
( smax , iq4v5bez02ka ( mmi , b_A , kend + 2 ) ) ; if ( smax >= 0.0 ) { scale
= - scale ; } tau -> data [ loop_ub ] = ( scale - smax ) / scale ; smax = 1.0
/ ( smax - scale ) ; iy = ( ( ( ( ( pvt - kend ) - 1 ) / 2 ) << 1 ) + kend )
+ 2 ; jy = iy - 2 ; for ( lastv = ix ; lastv <= jy ; lastv += 2 ) { tmp =
_mm_loadu_pd ( & b_A -> data [ lastv - 1 ] ) ; _mm_storeu_pd ( & b_A -> data
[ lastv - 1 ] , _mm_mul_pd ( tmp , _mm_set1_pd ( smax ) ) ) ; } for ( lastv =
iy ; lastv <= pvt ; lastv ++ ) { b_A -> data [ lastv - 1 ] *= smax ; } for (
lastv = 0 ; lastv <= knt ; lastv ++ ) { scale *= 1.0020841800044864E-292 ; }
smax = scale ; } else { tau -> data [ loop_ub ] = ( scale - b_A -> data [
kend ] ) / scale ; smax = 1.0 / ( b_A -> data [ kend ] - scale ) ; pvt = (
kend + mmi ) + 1 ; iy = ( ( ( ( ( pvt - kend ) - 1 ) / 2 ) << 1 ) + kend ) +
2 ; jy = iy - 2 ; for ( lastv = ix ; lastv <= jy ; lastv += 2 ) { tmp =
_mm_loadu_pd ( & b_A -> data [ lastv - 1 ] ) ; _mm_storeu_pd ( & b_A -> data
[ lastv - 1 ] , _mm_mul_pd ( tmp , _mm_set1_pd ( smax ) ) ) ; } for ( lastv =
iy ; lastv <= pvt ; lastv ++ ) { b_A -> data [ lastv - 1 ] *= smax ; } smax =
scale ; } } } b_A -> data [ kend ] = smax ; } else { tau -> data [ loop_ub ]
= 0.0 ; } if ( loop_ub + 1 < na ) { smax = b_A -> data [ kend ] ; b_A -> data
[ kend ] = 1.0 ; ix = ( kend + ma_p ) + 1 ; if ( tau -> data [ loop_ub ] !=
0.0 ) { lastv = mmi ; pvt = kend + mmi ; while ( ( lastv + 1 > 0 ) && ( b_A
-> data [ pvt ] == 0.0 ) ) { lastv -- ; pvt -- ; } exitg2 = false ; while ( (
! exitg2 ) && ( nmi + 1 > 0 ) ) { pvt = nmi * ma_p + ix ; jy = pvt ; do {
exitg1 = 0 ; if ( jy <= pvt + lastv ) { if ( b_A -> data [ jy - 1 ] != 0.0 )
{ exitg1 = 1 ; } else { jy ++ ; } } else { nmi -- ; exitg1 = 2 ; } } while (
exitg1 == 0 ) ; if ( exitg1 == 1 ) { exitg2 = true ; } } } else { lastv = - 1
; nmi = - 1 ; } if ( lastv + 1 > 0 ) { if ( nmi + 1 != 0 ) { if ( nmi >= 0 )
{ memset ( & work -> data [ 0 ] , 0 , ( uint32_T ) ( nmi + 1 ) * sizeof (
real_T ) ) ; } iy = 0 ; pvt = ma_p * nmi + ix ; for ( knt = ix ; ma_p < 0 ?
knt >= pvt : knt <= pvt ; knt += ma_p ) { scale = 0.0 ; d = knt + lastv ; for
( jy = knt ; jy <= d ; jy ++ ) { scale += b_A -> data [ ( kend + jy ) - knt ]
* b_A -> data [ jy - 1 ] ; } work -> data [ iy ] += scale ; iy ++ ; } } if (
! ( - tau -> data [ loop_ub ] == 0.0 ) ) { iy = ix ; for ( ix = 0 ; ix <= nmi
; ix ++ ) { scale = work -> data [ ix ] ; if ( scale != 0.0 ) { scale *= -
tau -> data [ loop_ub ] ; knt = lastv + iy ; for ( d = iy ; d <= knt ; d ++ )
{ b_A -> data [ d - 1 ] += b_A -> data [ ( kend + d ) - iy ] * scale ; } } iy
+= ma_p ; } } } b_A -> data [ kend ] = smax ; } for ( nmi = loop_ub + 2 ; nmi
<= na ; nmi ++ ) { kend = ( ( nmi - 1 ) * ma_p + loop_ub ) + 1 ; if ( vn1 ->
data [ nmi - 1 ] != 0.0 ) { smax = muDoubleScalarAbs ( b_A -> data [ kend - 1
] ) / vn1 -> data [ nmi - 1 ] ; smax = 1.0 - smax * smax ; if ( smax < 0.0 )
{ smax = 0.0 ; } scale = vn1 -> data [ nmi - 1 ] / vn2 -> data [ nmi - 1 ] ;
scale = scale * scale * smax ; if ( scale <= 1.4901161193847656E-8 ) { if (
loop_ub + 1 < ma ) { ix = kend ; smax = 0.0 ; if ( mmi < 1 ) { } else if (
mmi == 1 ) { smax = muDoubleScalarAbs ( b_A -> data [ kend ] ) ; } else {
scale = 3.3121686421112381E-170 ; kend += mmi ; for ( lastv = ix + 1 ; lastv
<= kend ; lastv ++ ) { absxk = muDoubleScalarAbs ( b_A -> data [ lastv - 1 ]
) ; if ( absxk > scale ) { t = scale / absxk ; smax = smax * t * t + 1.0 ;
scale = absxk ; } else { t = absxk / scale ; smax += t * t ; } } smax = scale
* muDoubleScalarSqrt ( smax ) ; } vn1 -> data [ nmi - 1 ] = smax ; vn2 ->
data [ nmi - 1 ] = vn1 -> data [ nmi - 1 ] ; } else { vn1 -> data [ nmi - 1 ]
= 0.0 ; vn2 -> data [ nmi - 1 ] = 0.0 ; } } else { vn1 -> data [ nmi - 1 ] *=
muDoubleScalarSqrt ( smax ) ; } } } } loop_ub = b_jpvt -> size [ 0 ] * b_jpvt
-> size [ 1 ] ; b_jpvt -> size [ 0 ] = 1 ; b_jpvt -> size [ 1 ] = b_jpvt_p ->
size [ 1 ] ; os5bv5kg0d ( b_jpvt , loop_ub ) ; loop_ub = b_jpvt_p -> size [ 1
] ; memcpy ( & b_jpvt -> data [ 0 ] , & b_jpvt_p -> data [ 0 ] , ( uint32_T )
loop_ub * sizeof ( int32_T ) ) ; } bdhoypqe5x ( & vn2 ) ; bdhoypqe5x ( & vn1
) ; bdhoypqe5x ( & work ) ; preyilpnl4 ( & b_jpvt_p ) ; loop_ub = jpvt ->
size [ 0 ] * jpvt -> size [ 1 ] ; jpvt -> size [ 0 ] = 1 ; jpvt -> size [ 1 ]
= b_jpvt -> size [ 1 ] ; os5bv5kg0d ( jpvt , loop_ub ) ; loop_ub = b_jpvt ->
size [ 1 ] ; if ( loop_ub - 1 >= 0 ) { memcpy ( & jpvt -> data [ 0 ] , &
b_jpvt -> data [ 0 ] , ( uint32_T ) loop_ub * sizeof ( int32_T ) ) ; }
preyilpnl4 ( & b_jpvt ) ; } static void dh3xcvj5bl ( int32_T m , int32_T n ,
const klgljlxbmf * A , int32_T lda , klgljlxbmf * b_A , ncvluzgkod * ipiv ,
int32_T * info ) { real_T s ; real_T smax ; int32_T c ; int32_T ijA ; int32_T
ix ; int32_T iy ; int32_T j ; int32_T jj ; int32_T loop_ub ; int32_T mmj ;
int32_T n_p ; int32_T yk ; yk = b_A -> size [ 0 ] * b_A -> size [ 1 ] ; b_A
-> size [ 0 ] = A -> size [ 0 ] ; b_A -> size [ 1 ] = A -> size [ 1 ] ;
miyl1rq4uj ( b_A , yk ) ; loop_ub = A -> size [ 0 ] * A -> size [ 1 ] ; if (
loop_ub - 1 >= 0 ) { memcpy ( & b_A -> data [ 0 ] , & A -> data [ 0 ] , (
uint32_T ) loop_ub * sizeof ( real_T ) ) ; } n_p = muIntScalarMin_sint32 ( m
, n ) ; if ( n_p < 1 ) { n_p = 0 ; } yk = ipiv -> size [ 0 ] * ipiv -> size [
1 ] ; ipiv -> size [ 0 ] = 1 ; ipiv -> size [ 1 ] = n_p ; os5bv5kg0d ( ipiv ,
yk ) ; if ( n_p > 0 ) { ipiv -> data [ 0 ] = 1 ; yk = 1 ; for ( ix = 2 ; ix
<= n_p ; ix ++ ) { yk ++ ; ipiv -> data [ ix - 1 ] = yk ; } } * info = 0 ; if
( ( m < 1 ) || ( n < 1 ) ) { } else { n_p = m - 1 ; n_p =
muIntScalarMin_sint32 ( n_p , n ) - 1 ; for ( yk = 0 ; yk <= n_p ; yk ++ ) {
mmj = m - yk ; loop_ub = ( lda + 1 ) * yk + 2 ; jj = ( lda + 1 ) * yk ; if (
mmj < 1 ) { iy = 0 ; } else { iy = 1 ; if ( mmj > 1 ) { smax =
muDoubleScalarAbs ( b_A -> data [ jj ] ) ; for ( ix = 2 ; ix <= mmj ; ix ++ )
{ s = muDoubleScalarAbs ( b_A -> data [ ( loop_ub + ix ) - 3 ] ) ; if ( s >
smax ) { iy = ix ; smax = s ; } } } } if ( b_A -> data [ ( loop_ub + iy ) - 3
] != 0.0 ) { if ( iy - 1 != 0 ) { ipiv -> data [ yk ] = yk + iy ; iy = ( yk +
iy ) - 1 ; for ( ix = 0 ; ix < n ; ix ++ ) { smax = b_A -> data [ ix * lda +
yk ] ; b_A -> data [ yk + ix * lda ] = b_A -> data [ ix * lda + iy ] ; b_A ->
data [ iy + ix * lda ] = smax ; } } iy = loop_ub + mmj ; for ( ix = loop_ub ;
ix <= iy - 2 ; ix ++ ) { b_A -> data [ ix - 1 ] /= b_A -> data [ jj ] ; } }
else { * info = yk + 1 ; } iy = ( n - yk ) - 1 ; ix = ( jj + lda ) + 1 ; jj
+= lda ; for ( j = 0 ; j < iy ; j ++ ) { smax = b_A -> data [ j * lda + jj ]
; if ( b_A -> data [ j * lda + jj ] != 0.0 ) { c = ( mmj + ix ) - 1 ; for (
ijA = ix + 1 ; ijA <= c ; ijA ++ ) { b_A -> data [ ijA - 1 ] += b_A -> data [
( ( loop_ub + ijA ) - ix ) - 2 ] * - smax ; } } ix += lda ; } } if ( ( * info
== 0 ) && ( m <= n ) && ( ! ( b_A -> data [ ( ( m - 1 ) * b_A -> size [ 0 ] +
m ) - 1 ] != 0.0 ) ) ) { * info = m ; } } } static void hjklbkk1g0 ( int32_T
m , int32_T n , const klgljlxbmf * A , int32_T lda , const klgljlxbmf * B_g ,
int32_T ldb , klgljlxbmf * b_B ) { int32_T b ; int32_T b_B_p ; int32_T b_i ;
int32_T jBcol ; int32_T kAcol ; int32_T loop_ub ; loop_ub = b_B -> size [ 0 ]
* b_B -> size [ 1 ] ; b_B -> size [ 0 ] = B_g -> size [ 0 ] ; b_B -> size [ 1
] = B_g -> size [ 1 ] ; miyl1rq4uj ( b_B , loop_ub ) ; loop_ub = B_g -> size
[ 0 ] * B_g -> size [ 1 ] ; if ( loop_ub - 1 >= 0 ) { memcpy ( & b_B -> data
[ 0 ] , & B_g -> data [ 0 ] , ( uint32_T ) loop_ub * sizeof ( real_T ) ) ; }
if ( ( n == 0 ) || ( ( B_g -> size [ 0 ] == 0 ) || ( B_g -> size [ 1 ] == 0 )
) ) { } else { for ( b_B_p = 0 ; b_B_p < n ; b_B_p ++ ) { jBcol = ldb * b_B_p
- 1 ; for ( loop_ub = m ; loop_ub >= 1 ; loop_ub -- ) { kAcol = ( loop_ub - 1
) * lda - 1 ; if ( b_B -> data [ loop_ub + jBcol ] != 0.0 ) { b_B -> data [
loop_ub + jBcol ] /= A -> data [ loop_ub + kAcol ] ; b = loop_ub - 2 ; for (
b_i = 0 ; b_i <= b ; b_i ++ ) { b_B -> data [ ( b_i + jBcol ) + 1 ] -= A ->
data [ ( b_i + kAcol ) + 1 ] * b_B -> data [ loop_ub + jBcol ] ; } } } } } }
static void cp5ozjiqhu ( const klgljlxbmf * A , const klgljlxbmf * B_i ,
klgljlxbmf * Y ) { __m128d tmp ; __m128d tmp_p ; klgljlxbmf * B_e ;
klgljlxbmf * b_A ; klgljlxbmf * tau ; ncvluzgkod * jpvt ; real_T tol ;
int32_T b_idx_1 ; int32_T m ; int32_T maxmn ; int32_T mb ; int32_T minmn ;
int32_T mn ; int32_T nb ; int32_T nb_p ; int32_T rankA ; int32_T scalarLB ;
int32_T vectorUB ; calykgtvmm ( & b_A , 2 ) ; calykgtvmm ( & tau , 1 ) ;
ks3ealo3sd ( & jpvt , 2 ) ; calykgtvmm ( & B_e , 2 ) ; if ( ( A -> size [ 0 ]
== 0 ) || ( A -> size [ 1 ] == 0 ) || ( ( B_i -> size [ 0 ] == 0 ) || ( B_i
-> size [ 1 ] == 0 ) ) ) { minmn = Y -> size [ 0 ] * Y -> size [ 1 ] ; Y ->
size [ 0 ] = A -> size [ 1 ] ; Y -> size [ 1 ] = B_i -> size [ 1 ] ;
miyl1rq4uj ( Y , minmn ) ; m = A -> size [ 1 ] * B_i -> size [ 1 ] ; if ( m -
1 >= 0 ) { memset ( & Y -> data [ 0 ] , 0 , ( uint32_T ) m * sizeof ( real_T
) ) ; } } else if ( A -> size [ 0 ] == A -> size [ 1 ] ) { minmn = A -> size
[ 0 ] ; maxmn = A -> size [ 1 ] ; mb = B_i -> size [ 0 ] ; maxmn =
muIntScalarMin_sint32 ( minmn , maxmn ) ; rankA = muIntScalarMin_sint32 ( mb
, maxmn ) ; nb = B_i -> size [ 1 ] - 1 ; dh3xcvj5bl ( rankA , rankA , A , A
-> size [ 0 ] , b_A , jpvt , & minmn ) ; minmn = B_e -> size [ 0 ] * B_e ->
size [ 1 ] ; B_e -> size [ 0 ] = B_i -> size [ 0 ] ; B_e -> size [ 1 ] = B_i
-> size [ 1 ] ; miyl1rq4uj ( B_e , minmn ) ; m = B_i -> size [ 0 ] * B_i ->
size [ 1 ] ; if ( m - 1 >= 0 ) { memcpy ( & B_e -> data [ 0 ] , & B_i -> data
[ 0 ] , ( uint32_T ) m * sizeof ( real_T ) ) ; } b_idx_1 = rankA - 2 ; for (
minmn = 0 ; minmn <= b_idx_1 ; minmn ++ ) { m = jpvt -> data [ minmn ] ; if (
minmn + 1 != m ) { for ( maxmn = 0 ; maxmn <= nb ; maxmn ++ ) { tol = B_e ->
data [ B_e -> size [ 0 ] * maxmn + minmn ] ; B_e -> data [ minmn + B_e ->
size [ 0 ] * maxmn ] = B_e -> data [ ( B_e -> size [ 0 ] * maxmn + m ) - 1 ]
; B_e -> data [ ( m + B_e -> size [ 0 ] * maxmn ) - 1 ] = tol ; } } } for (
maxmn = 0 ; maxmn <= nb ; maxmn ++ ) { m = B_i -> size [ 0 ] * maxmn - 1 ;
for ( mb = 0 ; mb < rankA ; mb ++ ) { mn = b_A -> size [ 0 ] * mb - 1 ; if (
B_e -> data [ ( mb + m ) + 1 ] != 0.0 ) { for ( minmn = mb + 2 ; minmn <=
rankA ; minmn ++ ) { B_e -> data [ minmn + m ] -= B_e -> data [ ( mb + m ) +
1 ] * b_A -> data [ minmn + mn ] ; } } } } hjklbkk1g0 ( rankA , B_i -> size [
1 ] , b_A , b_A -> size [ 0 ] , B_e , B_i -> size [ 0 ] , Y ) ; } else {
ldurgmcftq ( A , b_A , tau , jpvt ) ; rankA = 0 ; if ( b_A -> size [ 0 ] <
b_A -> size [ 1 ] ) { minmn = b_A -> size [ 0 ] ; maxmn = b_A -> size [ 1 ] ;
} else { minmn = b_A -> size [ 1 ] ; maxmn = b_A -> size [ 0 ] ; } if ( minmn
> 0 ) { tol = muDoubleScalarMin ( 1.4901161193847656E-8 ,
2.2204460492503131E-15 * ( real_T ) maxmn ) * muDoubleScalarAbs ( b_A -> data
[ 0 ] ) ; while ( ( rankA < minmn ) && ( ! ( muDoubleScalarAbs ( b_A -> data
[ b_A -> size [ 0 ] * rankA + rankA ] ) <= tol ) ) ) { rankA ++ ; } } nb =
B_i -> size [ 1 ] - 1 ; minmn = Y -> size [ 0 ] * Y -> size [ 1 ] ; Y -> size
[ 0 ] = b_A -> size [ 1 ] ; Y -> size [ 1 ] = B_i -> size [ 1 ] ; miyl1rq4uj
( Y , minmn ) ; m = b_A -> size [ 1 ] * B_i -> size [ 1 ] ; if ( m - 1 >= 0 )
{ memset ( & Y -> data [ 0 ] , 0 , ( uint32_T ) m * sizeof ( real_T ) ) ; }
minmn = B_e -> size [ 0 ] * B_e -> size [ 1 ] ; B_e -> size [ 0 ] = B_i ->
size [ 0 ] ; B_e -> size [ 1 ] = B_i -> size [ 1 ] ; miyl1rq4uj ( B_e , minmn
) ; m = B_i -> size [ 0 ] * B_i -> size [ 1 ] ; if ( m - 1 >= 0 ) { memcpy (
& B_e -> data [ 0 ] , & B_i -> data [ 0 ] , ( uint32_T ) m * sizeof ( real_T
) ) ; } m = b_A -> size [ 0 ] ; nb_p = B_i -> size [ 1 ] - 1 ; maxmn = b_A ->
size [ 0 ] ; b_idx_1 = b_A -> size [ 1 ] ; mn = muIntScalarMin_sint32 ( maxmn
, b_idx_1 ) - 1 ; for ( maxmn = 0 ; maxmn <= mn ; maxmn ++ ) { if ( tau ->
data [ maxmn ] != 0.0 ) { b_idx_1 = maxmn + 2 ; for ( mb = 0 ; mb <= nb_p ;
mb ++ ) { tol = B_e -> data [ B_e -> size [ 0 ] * mb + maxmn ] ; for ( minmn
= b_idx_1 ; minmn <= m ; minmn ++ ) { tol += b_A -> data [ ( b_A -> size [ 0
] * maxmn + minmn ) - 1 ] * B_e -> data [ ( B_e -> size [ 0 ] * mb + minmn )
- 1 ] ; } tol *= tau -> data [ maxmn ] ; if ( tol != 0.0 ) { B_e -> data [
maxmn + B_e -> size [ 0 ] * mb ] -= tol ; scalarLB = ( ( ( ( ( m - maxmn ) -
1 ) / 2 ) << 1 ) + maxmn ) + 2 ; vectorUB = scalarLB - 2 ; for ( minmn =
b_idx_1 ; minmn <= vectorUB ; minmn += 2 ) { tmp = _mm_loadu_pd ( & b_A ->
data [ ( b_A -> size [ 0 ] * maxmn + minmn ) - 1 ] ) ; tmp_p = _mm_loadu_pd (
& B_e -> data [ ( B_e -> size [ 0 ] * mb + minmn ) - 1 ] ) ; _mm_storeu_pd (
& B_e -> data [ ( minmn + B_e -> size [ 0 ] * mb ) - 1 ] , _mm_sub_pd ( tmp_p
, _mm_mul_pd ( tmp , _mm_set1_pd ( tol ) ) ) ) ; } for ( minmn = scalarLB ;
minmn <= m ; minmn ++ ) { B_e -> data [ ( minmn + B_e -> size [ 0 ] * mb ) -
1 ] -= b_A -> data [ ( b_A -> size [ 0 ] * maxmn + minmn ) - 1 ] * tol ; } }
} } } for ( mb = 0 ; mb <= nb ; mb ++ ) { for ( minmn = 0 ; minmn < rankA ;
minmn ++ ) { Y -> data [ ( jpvt -> data [ minmn ] + Y -> size [ 0 ] * mb ) -
1 ] = B_e -> data [ B_e -> size [ 0 ] * mb + minmn ] ; } for ( m = rankA ; m
>= 1 ; m -- ) { Y -> data [ ( jpvt -> data [ m - 1 ] + Y -> size [ 0 ] * mb )
- 1 ] /= b_A -> data [ ( ( m - 1 ) * b_A -> size [ 0 ] + m ) - 1 ] ; b_idx_1
= m - 2 ; for ( maxmn = 0 ; maxmn <= b_idx_1 ; maxmn ++ ) { Y -> data [ (
jpvt -> data [ maxmn ] + Y -> size [ 0 ] * mb ) - 1 ] -= Y -> data [ ( jpvt
-> data [ m - 1 ] + Y -> size [ 0 ] * mb ) - 1 ] * b_A -> data [ ( m - 1 ) *
b_A -> size [ 0 ] + maxmn ] ; } } } } bdhoypqe5x ( & B_e ) ; preyilpnl4 ( &
jpvt ) ; bdhoypqe5x ( & tau ) ; bdhoypqe5x ( & b_A ) ; } static void
naapodi4lu ( m5kgub1d5k * * pEmxArray ) { if ( * pEmxArray != ( m5kgub1d5k *
) NULL ) { if ( ( ( * pEmxArray ) -> data != ( boolean_T * ) NULL ) && ( *
pEmxArray ) -> canFreeData ) { free ( ( * pEmxArray ) -> data ) ; } free ( (
* pEmxArray ) -> size ) ; free ( * pEmxArray ) ; * pEmxArray = ( m5kgub1d5k *
) NULL ; } } static boolean_T aofopy35eo ( const ecfe5we0wc * obj , const
real_T Hg [ 6 ] , const klgljlxbmf * alpha ) { m5kgub1d5k * x ; int32_T ix ;
int32_T loop_ub ; boolean_T exitg1 ; boolean_T flag ; boolean_T y ;
l45e0kru51 ( & x , 1 ) ; if ( fdzsuttcyn1 ( Hg ) < obj -> GradientTolerance )
{ ix = x -> size [ 0 ] ; x -> size [ 0 ] = alpha -> size [ 0 ] ; dw1xs30knw (
x , ix ) ; loop_ub = alpha -> size [ 0 ] ; for ( ix = 0 ; ix < loop_ub ; ix
++ ) { x -> data [ ix ] = ( alpha -> data [ ix ] <= 0.0 ) ; } y = true ; ix =
0 ; exitg1 = false ; while ( ( ! exitg1 ) && ( ix + 1 <= x -> size [ 0 ] ) )
{ if ( ! x -> data [ ix ] ) { y = false ; exitg1 = true ; } else { ix ++ ; }
} if ( y ) { flag = true ; } else { flag = false ; } } else { flag = false ;
} naapodi4lu ( & x ) ; return flag ; } static void bqduk5sgwd ( const
klgljlxbmf * x , klgljlxbmf * y ) { klgljlxbmf * c_A ; klgljlxbmf * y_p ;
ncvluzgkod * b_ipiv ; ncvluzgkod * p ; int32_T b_ipiv_p ; int32_T n ; int32_T
n_p ; int32_T pipk ; int32_T yk ; if ( ( x -> size [ 0 ] == 0 ) || ( x ->
size [ 1 ] == 0 ) ) { pipk = y -> size [ 0 ] * y -> size [ 1 ] ; y -> size [
0 ] = x -> size [ 0 ] ; y -> size [ 1 ] = x -> size [ 1 ] ; miyl1rq4uj ( y ,
pipk ) ; pipk = x -> size [ 0 ] * x -> size [ 1 ] ; if ( pipk - 1 >= 0 ) {
memcpy ( & y -> data [ 0 ] , & x -> data [ 0 ] , ( uint32_T ) pipk * sizeof (
real_T ) ) ; } } else { n = x -> size [ 0 ] ; pipk = y -> size [ 0 ] * y ->
size [ 1 ] ; y -> size [ 0 ] = x -> size [ 0 ] ; y -> size [ 1 ] = x -> size
[ 1 ] ; miyl1rq4uj ( y , pipk ) ; n_p = x -> size [ 1 ] ; pipk = x -> size [
0 ] * x -> size [ 1 ] ; if ( pipk - 1 >= 0 ) { memset ( & y -> data [ 0 ] , 0
, ( uint32_T ) pipk * sizeof ( real_T ) ) ; } calykgtvmm ( & c_A , 2 ) ;
ks3ealo3sd ( & b_ipiv , 2 ) ; dh3xcvj5bl ( x -> size [ 0 ] , x -> size [ 0 ]
, x , x -> size [ 0 ] , c_A , b_ipiv , & n_p ) ; n_p = x -> size [ 0 ] ;
ks3ealo3sd ( & p , 2 ) ; pipk = p -> size [ 0 ] * p -> size [ 1 ] ; p -> size
[ 0 ] = 1 ; p -> size [ 1 ] = x -> size [ 0 ] ; os5bv5kg0d ( p , pipk ) ; p
-> data [ 0 ] = 1 ; yk = 1 ; for ( pipk = 2 ; pipk <= n_p ; pipk ++ ) { yk ++
; p -> data [ pipk - 1 ] = yk ; } n_p = b_ipiv -> size [ 1 ] - 1 ; for ( yk =
0 ; yk <= n_p ; yk ++ ) { b_ipiv_p = b_ipiv -> data [ yk ] ; if ( b_ipiv_p >
yk + 1 ) { pipk = p -> data [ b_ipiv_p - 1 ] ; p -> data [ b_ipiv_p - 1 ] = p
-> data [ yk ] ; p -> data [ yk ] = pipk ; } } preyilpnl4 ( & b_ipiv ) ; for
( yk = 0 ; yk < n ; yk ++ ) { n_p = p -> data [ yk ] ; y -> data [ yk + y ->
size [ 0 ] * ( n_p - 1 ) ] = 1.0 ; for ( pipk = yk + 1 ; pipk <= n ; pipk ++
) { if ( y -> data [ ( ( n_p - 1 ) * y -> size [ 0 ] + pipk ) - 1 ] != 0.0 )
{ for ( b_ipiv_p = pipk + 1 ; b_ipiv_p <= n ; b_ipiv_p ++ ) { y -> data [ (
b_ipiv_p + y -> size [ 0 ] * ( n_p - 1 ) ) - 1 ] = y -> data [ ( ( n_p - 1 )
* y -> size [ 0 ] + b_ipiv_p ) - 1 ] - y -> data [ ( ( n_p - 1 ) * y -> size
[ 0 ] + pipk ) - 1 ] * c_A -> data [ ( ( pipk - 1 ) * c_A -> size [ 0 ] +
b_ipiv_p ) - 1 ] ; } } } } preyilpnl4 ( & p ) ; calykgtvmm ( & y_p , 2 ) ;
pipk = y_p -> size [ 0 ] * y_p -> size [ 1 ] ; y_p -> size [ 0 ] = y -> size
[ 0 ] ; y_p -> size [ 1 ] = y -> size [ 1 ] ; miyl1rq4uj ( y_p , pipk ) ;
pipk = y -> size [ 0 ] * y -> size [ 1 ] - 1 ; if ( pipk >= 0 ) { memcpy ( &
y_p -> data [ 0 ] , & y -> data [ 0 ] , ( uint32_T ) ( pipk + 1 ) * sizeof (
real_T ) ) ; } hjklbkk1g0 ( x -> size [ 0 ] , x -> size [ 0 ] , c_A , x ->
size [ 0 ] , y_p , x -> size [ 0 ] , y ) ; bdhoypqe5x ( & y_p ) ; bdhoypqe5x
( & c_A ) ; } } static void gka31aeazw ( const klgljlxbmf * v , klgljlxbmf *
d ) { int32_T m ; int32_T n ; if ( ( v -> size [ 0 ] == 1 ) && ( v -> size [
1 ] == 1 ) ) { n = d -> size [ 0 ] ; d -> size [ 0 ] = 1 ; miyl1rq4uj ( d , n
) ; d -> data [ 0 ] = v -> data [ 0 ] ; } else { m = v -> size [ 0 ] ; n = v
-> size [ 1 ] ; if ( v -> size [ 1 ] > 0 ) { m = muIntScalarMin_sint32 ( m ,
n ) ; } else { m = 0 ; } n = d -> size [ 0 ] ; d -> size [ 0 ] = m ;
miyl1rq4uj ( d , n ) ; for ( n = 0 ; n < m ; n ++ ) { d -> data [ n ] = v ->
data [ v -> size [ 0 ] * n + n ] ; } } } static void dmv3200pfy ( const
klgljlxbmf * x , real_T * ex , int32_T * idx ) { int32_T b ; int32_T k ;
int32_T last ; boolean_T exitg1 ; last = x -> size [ 0 ] ; if ( x -> size [ 0
] <= 2 ) { if ( x -> size [ 0 ] == 1 ) { * ex = x -> data [ 0 ] ; * idx = 1 ;
} else if ( ( x -> data [ 0 ] < x -> data [ x -> size [ 0 ] - 1 ] ) || (
muDoubleScalarIsNaN ( x -> data [ 0 ] ) && ( ! muDoubleScalarIsNaN ( x ->
data [ x -> size [ 0 ] - 1 ] ) ) ) ) { * ex = x -> data [ x -> size [ 0 ] - 1
] ; * idx = x -> size [ 0 ] ; } else { * ex = x -> data [ 0 ] ; * idx = 1 ; }
} else { if ( ! muDoubleScalarIsNaN ( x -> data [ 0 ] ) ) { * idx = 1 ; }
else { * idx = 0 ; k = 2 ; exitg1 = false ; while ( ( ! exitg1 ) && ( k <=
last ) ) { if ( ! muDoubleScalarIsNaN ( x -> data [ k - 1 ] ) ) { * idx = k ;
exitg1 = true ; } else { k ++ ; } } } if ( * idx == 0 ) { * ex = x -> data [
0 ] ; * idx = 1 ; } else { * ex = x -> data [ * idx - 1 ] ; b = * idx ; for (
k = b + 1 ; k <= last ; k ++ ) { if ( * ex < x -> data [ k - 1 ] ) { * ex = x
-> data [ k - 1 ] ; * idx = k ; } } } } } static void offa2br1khoeg ( const
klgljlxbmf * in1 , const klgljlxbmf * in2 , real_T * out1 , int32_T * out2 )
{ klgljlxbmf * in1_p ; int32_T i ; int32_T loop_ub ; int32_T stride_0_0 ;
int32_T stride_1_0 ; calykgtvmm ( & in1_p , 1 ) ; i = in1_p -> size [ 0 ] ;
in1_p -> size [ 0 ] = in2 -> size [ 0 ] == 1 ? in1 -> size [ 0 ] : in2 ->
size [ 0 ] ; miyl1rq4uj ( in1_p , i ) ; stride_0_0 = ( in1 -> size [ 0 ] != 1
) ; stride_1_0 = ( in2 -> size [ 0 ] != 1 ) ; loop_ub = in2 -> size [ 0 ] ==
1 ? in1 -> size [ 0 ] : in2 -> size [ 0 ] ; for ( i = 0 ; i < loop_ub ; i ++
) { in1_p -> data [ i ] = in1 -> data [ i * stride_0_0 ] / in2 -> data [ i *
stride_1_0 ] ; } dmv3200pfy ( in1_p , out1 , out2 ) ; bdhoypqe5x ( & in1_p )
; } static boolean_T pahyhnfmn2 ( const m5kgub1d5k * x ) { int32_T ix ;
boolean_T exitg1 ; boolean_T y ; y = false ; ix = 0 ; exitg1 = false ; while
( ( ! exitg1 ) && ( ix + 1 <= x -> size [ 0 ] ) ) { if ( x -> data [ ix ] ) {
y = true ; exitg1 = true ; } else { ix ++ ; } } return y ; } static void
offa2br1khoe ( klgljlxbmf * in1 , const klgljlxbmf * in2 , const klgljlxbmf *
in3 ) { klgljlxbmf * in1_p ; int32_T i ; int32_T loop_ub ; int32_T stride_0_0
; int32_T stride_1_0 ; int32_T stride_2_0 ; calykgtvmm ( & in1_p , 1 ) ; i =
in1_p -> size [ 0 ] ; in1_p -> size [ 0 ] = in3 -> size [ 0 ] == 1 ? in2 ->
size [ 0 ] == 1 ? in1 -> size [ 0 ] : in2 -> size [ 0 ] : in3 -> size [ 0 ] ;
miyl1rq4uj ( in1_p , i ) ; stride_0_0 = ( in1 -> size [ 0 ] != 1 ) ;
stride_1_0 = ( in2 -> size [ 0 ] != 1 ) ; stride_2_0 = ( in3 -> size [ 0 ] !=
1 ) ; loop_ub = in3 -> size [ 0 ] == 1 ? in2 -> size [ 0 ] == 1 ? in1 -> size
[ 0 ] : in2 -> size [ 0 ] : in3 -> size [ 0 ] ; for ( i = 0 ; i < loop_ub ; i
++ ) { in1_p -> data [ i ] = ( in1 -> data [ i * stride_0_0 ] - in2 -> data [
i * stride_1_0 ] ) / in3 -> data [ i * stride_2_0 ] ; } i = in1 -> size [ 0 ]
; in1 -> size [ 0 ] = in1_p -> size [ 0 ] ; miyl1rq4uj ( in1 , i ) ; loop_ub
= in1_p -> size [ 0 ] ; if ( loop_ub - 1 >= 0 ) { memcpy ( & in1 -> data [ 0
] , & in1_p -> data [ 0 ] , ( uint32_T ) loop_ub * sizeof ( real_T ) ) ; }
bdhoypqe5x ( & in1_p ) ; } static void mfmaegtoxtz ( const m5kgub1d5k * x ,
ncvluzgkod * i ) { int32_T b_ii ; int32_T idx ; int32_T nx ; boolean_T exitg1
; nx = x -> size [ 0 ] ; idx = 0 ; b_ii = i -> size [ 0 ] ; i -> size [ 0 ] =
x -> size [ 0 ] ; os5bv5kg0d ( i , b_ii ) ; b_ii = 1 ; exitg1 = false ; while
( ( ! exitg1 ) && ( b_ii - 1 <= nx - 1 ) ) { if ( x -> data [ b_ii - 1 ] ) {
idx ++ ; i -> data [ idx - 1 ] = b_ii ; if ( idx >= nx ) { exitg1 = true ; }
else { b_ii ++ ; } } else { b_ii ++ ; } } if ( x -> size [ 0 ] == 1 ) { if (
idx == 0 ) { i -> size [ 0 ] = 0 ; } } else { if ( idx < 1 ) { idx = 0 ; }
b_ii = i -> size [ 0 ] ; i -> size [ 0 ] = idx ; os5bv5kg0d ( i , b_ii ) ; }
} static void heltobv2ai ( const klgljlxbmf * x , real_T * ex , int32_T * idx
) { int32_T b ; int32_T k ; int32_T last ; boolean_T exitg1 ; last = x ->
size [ 0 ] ; if ( x -> size [ 0 ] <= 2 ) { if ( x -> size [ 0 ] == 1 ) { * ex
= x -> data [ 0 ] ; * idx = 1 ; } else if ( ( x -> data [ 0 ] > x -> data [ x
-> size [ 0 ] - 1 ] ) || ( muDoubleScalarIsNaN ( x -> data [ 0 ] ) && ( !
muDoubleScalarIsNaN ( x -> data [ x -> size [ 0 ] - 1 ] ) ) ) ) { * ex = x ->
data [ x -> size [ 0 ] - 1 ] ; * idx = x -> size [ 0 ] ; } else { * ex = x ->
data [ 0 ] ; * idx = 1 ; } } else { if ( ! muDoubleScalarIsNaN ( x -> data [
0 ] ) ) { * idx = 1 ; } else { * idx = 0 ; k = 2 ; exitg1 = false ; while ( (
! exitg1 ) && ( k <= last ) ) { if ( ! muDoubleScalarIsNaN ( x -> data [ k -
1 ] ) ) { * idx = k ; exitg1 = true ; } else { k ++ ; } } } if ( * idx == 0 )
{ * ex = x -> data [ 0 ] ; * idx = 1 ; } else { * ex = x -> data [ * idx - 1
] ; b = * idx ; for ( k = b + 1 ; k <= last ; k ++ ) { if ( * ex > x -> data
[ k - 1 ] ) { * ex = x -> data [ k - 1 ] ; * idx = k ; } } } } } static void
bsiqjn4ttti1 ( klgljlxbmf * in1 , const klgljlxbmf * in2 ) { klgljlxbmf *
in2_p ; int32_T i ; int32_T loop_ub ; int32_T stride_0_0 ; int32_T stride_1_0
; calykgtvmm ( & in2_p , 1 ) ; i = in2_p -> size [ 0 ] ; in2_p -> size [ 0 ]
= in1 -> size [ 0 ] == 1 ? in2 -> size [ 0 ] : in1 -> size [ 0 ] ; miyl1rq4uj
( in2_p , i ) ; stride_0_0 = ( in2 -> size [ 0 ] != 1 ) ; stride_1_0 = ( in1
-> size [ 0 ] != 1 ) ; loop_ub = in1 -> size [ 0 ] == 1 ? in2 -> size [ 0 ] :
in1 -> size [ 0 ] ; for ( i = 0 ; i < loop_ub ; i ++ ) { in2_p -> data [ i ]
= in2 -> data [ i * stride_0_0 ] - in1 -> data [ i * stride_1_0 ] ; } i = in1
-> size [ 0 ] ; in1 -> size [ 0 ] = in2_p -> size [ 0 ] ; miyl1rq4uj ( in1 ,
i ) ; loop_ub = in2_p -> size [ 0 ] ; if ( loop_ub - 1 >= 0 ) { memcpy ( &
in1 -> data [ 0 ] , & in2_p -> data [ 0 ] , ( uint32_T ) loop_ub * sizeof (
real_T ) ) ; } bdhoypqe5x ( & in2_p ) ; } static void offa2br1kho ( real_T
in1 [ 36 ] , const real_T in2 [ 36 ] , const real_T in3 [ 6 ] , const
klgljlxbmf * in4 , real_T in5 ) { __m128d tmp ; __m128d tmp_p ; klgljlxbmf *
in3_p ; int32_T aux_0_1 ; int32_T i ; int32_T i_p ; int32_T loop_ub ;
calykgtvmm ( & in3_p , 2 ) ; i_p = in3_p -> size [ 0 ] * in3_p -> size [ 1 ]
; in3_p -> size [ 0 ] = 6 ; in3_p -> size [ 1 ] = in4 -> size [ 0 ] ;
miyl1rq4uj ( in3_p , i_p ) ; loop_ub = in4 -> size [ 0 ] ; for ( i_p = 0 ;
i_p < loop_ub ; i_p ++ ) { for ( i = 0 ; i <= 4 ; i += 2 ) { tmp_p =
_mm_loadu_pd ( & in3 [ i ] ) ; _mm_storeu_pd ( & in3_p -> data [ i + 6 * i_p
] , _mm_div_pd ( _mm_mul_pd ( tmp_p , _mm_set1_pd ( in4 -> data [ i_p ] ) ) ,
_mm_set1_pd ( in5 ) ) ) ; } } loop_ub = ( in3_p -> size [ 1 ] != 1 ) ;
aux_0_1 = 0 ; for ( i_p = 0 ; i_p < 6 ; i_p ++ ) { for ( i = 0 ; i <= 4 ; i
+= 2 ) { tmp_p = _mm_loadu_pd ( & in2 [ 6 * i_p + i ] ) ; tmp = _mm_loadu_pd
( & in3_p -> data [ 6 * aux_0_1 + i ] ) ; _mm_storeu_pd ( & in1 [ i + 6 * i_p
] , _mm_sub_pd ( tmp_p , tmp ) ) ; } aux_0_1 += loop_ub ; } bdhoypqe5x ( &
in3_p ) ; } static boolean_T np5inobdqs ( const real_T B_f [ 36 ] ) { __m128d
tmp ; real_T c_A [ 36 ] ; real_T c ; real_T ssq ; int32_T b ; int32_T b_info
; int32_T b_j ; int32_T d ; int32_T ia ; int32_T iac ; int32_T idxAjj ;
int32_T idxAjp11 ; boolean_T exitg1 ; memcpy ( & c_A [ 0 ] , & B_f [ 0 ] ,
36U * sizeof ( real_T ) ) ; b_info = 0 ; b_j = 1 ; exitg1 = false ; while ( (
! exitg1 ) && ( b_j - 1 < 6 ) ) { idxAjj = ( ( b_j - 1 ) * 6 + b_j ) - 1 ;
ssq = 0.0 ; if ( b_j - 1 >= 1 ) { b = ( uint8_T ) ( b_j - 1 ) ; for (
idxAjp11 = 0 ; idxAjp11 < b ; idxAjp11 ++ ) { ssq += c_A [ ( idxAjp11 * 6 +
b_j ) - 1 ] * c_A [ ( idxAjp11 * 6 + b_j ) - 1 ] ; } } ssq = c_A [ idxAjj ] -
ssq ; if ( ssq > 0.0 ) { ssq = muDoubleScalarSqrt ( ssq ) ; c_A [ idxAjj ] =
ssq ; if ( b_j < 6 ) { idxAjp11 = b_j + 1 ; if ( b_j - 1 != 0 ) { b = ( b_j -
2 ) * 6 + b_j ; for ( iac = idxAjp11 ; iac <= b + 1 ; iac += 6 ) { c = - c_A
[ ( div_nde_s32_floor ( ( iac - b_j ) - 1 , 6 ) * 6 + b_j ) - 1 ] ; d = iac -
b_j ; for ( ia = iac ; ia <= d + 5 ; ia ++ ) { c_A [ ( ( idxAjj + ia ) - iac
) + 1 ] += c_A [ ia - 1 ] * c ; } } } ssq = 1.0 / ssq ; b = idxAjj - b_j ;
iac = ( ( ( ( ( b - idxAjj ) + 6 ) / 2 ) << 1 ) + idxAjj ) + 2 ; d = iac - 2
; for ( idxAjp11 = idxAjj + 2 ; idxAjp11 <= d ; idxAjp11 += 2 ) { tmp =
_mm_loadu_pd ( & c_A [ idxAjp11 - 1 ] ) ; _mm_storeu_pd ( & c_A [ idxAjp11 -
1 ] , _mm_mul_pd ( tmp , _mm_set1_pd ( ssq ) ) ) ; } for ( idxAjp11 = iac ;
idxAjp11 <= b + 7 ; idxAjp11 ++ ) { c_A [ idxAjp11 - 1 ] *= ssq ; } } b_j ++
; } else { b_info = b_j ; exitg1 = true ; } } return b_info == 0 ; } static
void l5ks21uytpaq2zf ( const klgljlxbmf * A , const real_T B_j [ 36 ] ,
klgljlxbmf * C ) { __m128d tmp ; real_T bkj ; int32_T aoffset ; int32_T b_i ;
int32_T b_j ; int32_T boffset ; int32_T coffset ; int32_T inner ; int32_T k ;
int32_T m ; int32_T scalarLB ; int32_T vectorUB ; m = A -> size [ 0 ] - 1 ;
inner = A -> size [ 1 ] - 1 ; b_j = C -> size [ 0 ] * C -> size [ 1 ] ; C ->
size [ 0 ] = A -> size [ 0 ] ; C -> size [ 1 ] = 6 ; miyl1rq4uj ( C , b_j ) ;
for ( b_j = 0 ; b_j < 6 ; b_j ++ ) { coffset = ( m + 1 ) * b_j - 1 ; boffset
= b_j * 6 - 1 ; if ( m >= 0 ) { memset ( & C -> data [ coffset + 1 ] , 0 , (
uint32_T ) ( ( ( m + coffset ) - coffset ) + 1 ) * sizeof ( real_T ) ) ; }
for ( b_i = 0 ; b_i <= inner ; b_i ++ ) { aoffset = b_i * A -> size [ 0 ] - 1
; bkj = B_j [ ( boffset + b_i ) + 1 ] ; scalarLB = ( ( m + 1 ) / 2 ) << 1 ;
vectorUB = scalarLB - 2 ; for ( k = 0 ; k <= vectorUB ; k += 2 ) { tmp =
_mm_loadu_pd ( & C -> data [ ( k + coffset ) + 1 ] ) ; _mm_storeu_pd ( & C ->
data [ ( coffset + k ) + 1 ] , _mm_add_pd ( _mm_mul_pd ( _mm_loadu_pd ( & A
-> data [ ( k + aoffset ) + 1 ] ) , _mm_set1_pd ( bkj ) ) , tmp ) ) ; } for (
k = scalarLB ; k <= m ; k ++ ) { C -> data [ ( coffset + k ) + 1 ] += A ->
data [ ( k + aoffset ) + 1 ] * bkj ; } } } } static void l5ks21uytpaq2z (
const klgljlxbmf * A , const klgljlxbmf * B_p , klgljlxbmf * C ) { __m128d
tmp ; real_T bkj ; int32_T aoffset ; int32_T b_i ; int32_T b_j ; int32_T
boffset ; int32_T coffset ; int32_T inner ; int32_T k ; int32_T m ; int32_T n
; int32_T scalarLB ; int32_T vectorUB ; m = A -> size [ 0 ] - 1 ; inner = A
-> size [ 1 ] - 1 ; n = B_p -> size [ 1 ] - 1 ; b_j = C -> size [ 0 ] * C ->
size [ 1 ] ; C -> size [ 0 ] = A -> size [ 0 ] ; C -> size [ 1 ] = B_p ->
size [ 1 ] ; miyl1rq4uj ( C , b_j ) ; for ( b_j = 0 ; b_j <= n ; b_j ++ ) {
coffset = ( m + 1 ) * b_j - 1 ; boffset = b_j * B_p -> size [ 0 ] - 1 ; if (
m >= 0 ) { memset ( & C -> data [ coffset + 1 ] , 0 , ( uint32_T ) ( ( ( m +
coffset ) - coffset ) + 1 ) * sizeof ( real_T ) ) ; } for ( b_i = 0 ; b_i <=
inner ; b_i ++ ) { aoffset = b_i * A -> size [ 0 ] - 1 ; bkj = B_p -> data [
( boffset + b_i ) + 1 ] ; scalarLB = ( ( m + 1 ) / 2 ) << 1 ; vectorUB =
scalarLB - 2 ; for ( k = 0 ; k <= vectorUB ; k += 2 ) { tmp = _mm_loadu_pd (
& C -> data [ ( k + coffset ) + 1 ] ) ; _mm_storeu_pd ( & C -> data [ (
coffset + k ) + 1 ] , _mm_add_pd ( _mm_mul_pd ( _mm_loadu_pd ( & A -> data [
( k + aoffset ) + 1 ] ) , _mm_set1_pd ( bkj ) ) , tmp ) ) ; } for ( k =
scalarLB ; k <= m ; k ++ ) { C -> data [ ( coffset + k ) + 1 ] += A -> data [
( k + aoffset ) + 1 ] * bkj ; } } } } static void bsiqjn4ttti ( real_T in1 [
36 ] , const klgljlxbmf * in2 ) { real_T in1_p [ 36 ] ; int32_T aux_0_1 ;
int32_T i ; int32_T i_p ; int32_T stride_0_0 ; int32_T stride_0_1 ;
stride_0_0 = ( in2 -> size [ 0 ] != 1 ) ; stride_0_1 = ( in2 -> size [ 1 ] !=
1 ) ; aux_0_1 = 0 ; for ( i_p = 0 ; i_p < 6 ; i_p ++ ) { for ( i = 0 ; i < 6
; i ++ ) { in1_p [ i + 6 * i_p ] = in1 [ 6 * i_p + i ] - in2 -> data [ i *
stride_0_0 + in2 -> size [ 0 ] * aux_0_1 ] ; } aux_0_1 += stride_0_1 ; }
memcpy ( & in1 [ 0 ] , & in1_p [ 0 ] , 36U * sizeof ( real_T ) ) ; } static
void ngq4y5c0bx ( ecfe5we0wc * obj , real_T xSol [ 6 ] , gkb0igonbu *
exitFlag , real_T * err , real_T * iter ) { __m128d tmp_e ; __m128d tmp_i ;
__m128d tmp_p ; klgljlxbmf * A ; klgljlxbmf * AIn ; klgljlxbmf * A_p ;
klgljlxbmf * L ; klgljlxbmf * a__2 ; klgljlxbmf * alpha ; klgljlxbmf *
alpha_p ; klgljlxbmf * grad ; klgljlxbmf * tmp ; lnin0tndd2 * b ; m5kgub1d5k
* activeSet ; m5kgub1d5k * activeSet_p ; ncvluzgkod * bb ; ncvluzgkod * cb ;
ncvluzgkod * db ; ncvluzgkod * eb ; ncvluzgkod * fb ; ncvluzgkod * gb ;
real_T H [ 36 ] ; real_T H_p [ 36 ] ; real_T P_p [ 36 ] ; real_T V [ 36 ] ;
real_T idxl_p [ 36 ] ; real_T Hg [ 6 ] ; real_T grad_p [ 6 ] ; real_T sNew [
6 ] ; real_T x [ 6 ] ; real_T A_e ; real_T b_gamma ; real_T beta ; real_T
cost ; real_T costNew ; real_T idxl ; real_T lambda ; real_T m ; real_T sigma
; real_T sigma_p ; int32_T aoffset ; int32_T b_k ; int32_T g_idx_0 ; int32_T
i ; int32_T inner ; int32_T loop_ub ; int32_T nx ; int32_T scalarLB ; int32_T
vectorUB ; boolean_T flag ; static const int8_T tmp_m [ 36 ] = { 1 , 0 , 0 ,
0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 0 , 0 , 1 , 0
, 0 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 0 , 0 , 1 } ; klgljlxbmf * tmp_b ;
klgljlxbmf * tmp_c ; klgljlxbmf * tmp_f ; klgljlxbmf * tmp_j ; klgljlxbmf *
tmp_k ; ncvluzgkod * tmp_g ; int32_T exitg1 ; int32_T exitg2 ; boolean_T
guard1 = false ; boolean_T guard2 = false ; boolean_T guard3 = false ; for (
i = 0 ; i < 6 ; i ++ ) { x [ i ] = obj -> SeedInternal [ i ] ; } ot2wucsrdj (
& obj -> TimeObjInternal . StartTime . tv_sec , & obj -> TimeObjInternal .
StartTime . tv_nsec ) ; calykgtvmm ( & a__2 , 2 ) ; pbvdpfzges ( x , obj ->
ExtraArgs , & cost , H , a__2 , & b ) ; obj -> ExtraArgs = b ; b = obj ->
ExtraArgs ; calykgtvmm ( & grad , 1 ) ; aoffset = grad -> size [ 0 ] ; grad
-> size [ 0 ] = b -> GradTemp -> size [ 0 ] ; miyl1rq4uj ( grad , aoffset ) ;
inner = b -> GradTemp -> size [ 0 ] ; for ( i = 0 ; i < inner ; i ++ ) { grad
-> data [ i ] = b -> GradTemp -> data [ i ] ; } memset ( & H [ 0 ] , 0 , 36U
* sizeof ( real_T ) ) ; for ( b_k = 0 ; b_k < 6 ; b_k ++ ) { H [ b_k + 6 *
b_k ] = 1.0 ; } l45e0kru51 ( & activeSet , 1 ) ; calykgtvmm ( & A , 2 ) ;
calykgtvmm ( & L , 1 ) ; ks3ealo3sd ( & bb , 1 ) ; if ( obj -> ConstraintsOn
) { aoffset = A -> size [ 0 ] * A -> size [ 1 ] ; A -> size [ 0 ] = obj ->
ConstraintMatrix -> size [ 0 ] ; A -> size [ 1 ] = obj -> ConstraintMatrix ->
size [ 1 ] ; miyl1rq4uj ( A , aoffset ) ; inner = obj -> ConstraintMatrix ->
size [ 0 ] * obj -> ConstraintMatrix -> size [ 1 ] ; for ( i = 0 ; i < inner
; i ++ ) { A -> data [ i ] = obj -> ConstraintMatrix -> data [ i ] ; }
l5ks21uytpa ( A , x , L ) ; if ( L -> size [ 0 ] == obj -> ConstraintBound ->
size [ 0 ] ) { aoffset = activeSet -> size [ 0 ] ; activeSet -> size [ 0 ] =
L -> size [ 0 ] ; dw1xs30knw ( activeSet , aoffset ) ; inner = L -> size [ 0
] ; for ( i = 0 ; i < inner ; i ++ ) { activeSet -> data [ i ] = ( L -> data
[ i ] >= obj -> ConstraintBound -> data [ i ] ) ; } } else { offa2br1khoegk (
activeSet , L , obj ) ; } b_k = activeSet -> size [ 0 ] - 1 ; nx = 0 ; for (
g_idx_0 = 0 ; g_idx_0 <= b_k ; g_idx_0 ++ ) { if ( activeSet -> data [
g_idx_0 ] ) { nx ++ ; } } aoffset = bb -> size [ 0 ] ; bb -> size [ 0 ] = nx
; os5bv5kg0d ( bb , aoffset ) ; nx = 0 ; for ( g_idx_0 = 0 ; g_idx_0 <= b_k ;
g_idx_0 ++ ) { if ( activeSet -> data [ g_idx_0 ] ) { bb -> data [ nx ] =
g_idx_0 + 1 ; nx ++ ; } } aoffset = A -> size [ 0 ] * A -> size [ 1 ] ; A ->
size [ 0 ] = obj -> ConstraintMatrix -> size [ 0 ] ; A -> size [ 1 ] = bb ->
size [ 0 ] ; miyl1rq4uj ( A , aoffset ) ; inner = bb -> size [ 0 ] ; for ( i
= 0 ; i < inner ; i ++ ) { loop_ub = obj -> ConstraintMatrix -> size [ 0 ] ;
for ( aoffset = 0 ; aoffset < loop_ub ; aoffset ++ ) { A -> data [ aoffset +
A -> size [ 0 ] * i ] = obj -> ConstraintMatrix -> data [ ( bb -> data [ i ]
- 1 ) * obj -> ConstraintMatrix -> size [ 0 ] + aoffset ] ; } } } else {
g_idx_0 = obj -> ConstraintBound -> size [ 0 ] ; aoffset = activeSet -> size
[ 0 ] ; activeSet -> size [ 0 ] = g_idx_0 ; dw1xs30knw ( activeSet , aoffset
) ; if ( g_idx_0 - 1 >= 0 ) { memset ( & activeSet -> data [ 0 ] , 0 , (
uint32_T ) g_idx_0 * sizeof ( boolean_T ) ) ; } A -> size [ 0 ] = 6 ; A ->
size [ 1 ] = 0 ; } g_idx_0 = A -> size [ 1 ] - 1 ; calykgtvmm ( & A_p , 2 ) ;
for ( b_k = 0 ; b_k <= g_idx_0 ; b_k ++ ) { A_e = 0.0 ; for ( i = 0 ; i < 6 ;
i ++ ) { Hg [ i ] = 0.0 ; for ( aoffset = 0 ; aoffset < 6 ; aoffset ++ ) { Hg
[ i ] += H [ 6 * i + aoffset ] * A -> data [ A -> size [ 0 ] * b_k + aoffset
] ; } A_e += A -> data [ A -> size [ 0 ] * b_k + i ] * Hg [ i ] ; } idxl =
1.0 / A_e ; for ( i = 0 ; i <= 34 ; i += 2 ) { tmp_i = _mm_loadu_pd ( & H [ i
] ) ; _mm_storeu_pd ( & idxl_p [ i ] , _mm_mul_pd ( _mm_set1_pd ( idxl ) ,
tmp_i ) ) ; } aoffset = A_p -> size [ 0 ] * A_p -> size [ 1 ] ; A_p -> size [
0 ] = A -> size [ 0 ] ; A_p -> size [ 1 ] = A -> size [ 0 ] ; miyl1rq4uj (
A_p , aoffset ) ; inner = A -> size [ 0 ] ; for ( i = 0 ; i < inner ; i ++ )
{ loop_ub = A -> size [ 0 ] ; scalarLB = ( A -> size [ 0 ] / 2 ) << 1 ;
vectorUB = scalarLB - 2 ; for ( aoffset = 0 ; aoffset <= vectorUB ; aoffset
+= 2 ) { tmp_i = _mm_loadu_pd ( & A -> data [ A -> size [ 0 ] * b_k + aoffset
] ) ; _mm_storeu_pd ( & A_p -> data [ aoffset + A_p -> size [ 0 ] * i ] ,
_mm_mul_pd ( tmp_i , _mm_set1_pd ( A -> data [ i + A -> size [ 0 ] * b_k ] )
) ) ; } for ( aoffset = scalarLB ; aoffset < loop_ub ; aoffset ++ ) { A_p ->
data [ aoffset + A_p -> size [ 0 ] * i ] = A -> data [ A -> size [ 0 ] * b_k
+ aoffset ] * A -> data [ A -> size [ 0 ] * b_k + i ] ; } } l5ks21uytpaq (
idxl_p , A_p , a__2 ) ; for ( i = 0 ; i < 6 ; i ++ ) { for ( aoffset = 0 ;
aoffset < 6 ; aoffset ++ ) { idxl = 0.0 ; for ( inner = 0 ; inner < 6 ; inner
++ ) { idxl += a__2 -> data [ 6 * inner + i ] * H [ 6 * aoffset + inner ] ; }
H_p [ i + 6 * aoffset ] = H [ 6 * aoffset + i ] - idxl ; } } memcpy ( & H [ 0
] , & H_p [ 0 ] , 36U * sizeof ( real_T ) ) ; } for ( i = 0 ; i < 6 ; i ++ )
{ xSol [ i ] = x [ i ] ; } A_e = obj -> MaxNumIterationInternal ; g_idx_0 = 0
; calykgtvmm ( & alpha , 1 ) ; calykgtvmm ( & AIn , 2 ) ; ks3ealo3sd ( & cb ,
1 ) ; ks3ealo3sd ( & db , 1 ) ; ks3ealo3sd ( & eb , 1 ) ; ks3ealo3sd ( & fb ,
1 ) ; ks3ealo3sd ( & gb , 1 ) ; calykgtvmm ( & tmp , 2 ) ; l45e0kru51 ( &
activeSet_p , 1 ) ; calykgtvmm ( & alpha_p , 1 ) ; ks3ealo3sd ( & tmp_g , 1 )
; calykgtvmm ( & tmp_j , 2 ) ; calykgtvmm ( & tmp_f , 2 ) ; calykgtvmm ( &
tmp_c , 2 ) ; calykgtvmm ( & tmp_k , 2 ) ; calykgtvmm ( & tmp_b , 2 ) ; do {
exitg2 = 0 ; if ( g_idx_0 <= ( int32_T ) A_e - 1 ) { costNew = ethycgs32k (
obj -> TimeObjInternal . StartTime . tv_sec , obj -> TimeObjInternal .
StartTime . tv_nsec ) ; flag = ( costNew > obj -> MaxTimeInternal ) ; if (
flag ) { * exitFlag =
robotics_core_internal_NLPSolverExitFlags_TimeLimitExceeded ; b = obj ->
ExtraArgs ; for ( i = 0 ; i < 36 ; i ++ ) { H [ i ] = b -> WeightMatrix [ i ]
; } aoffset = grad -> size [ 0 ] ; grad -> size [ 0 ] = b -> ErrTemp -> size
[ 0 ] ; miyl1rq4uj ( grad , aoffset ) ; inner = b -> ErrTemp -> size [ 0 ] ;
for ( i = 0 ; i < inner ; i ++ ) { grad -> data [ i ] = b -> ErrTemp -> data
[ i ] ; } for ( i = 0 ; i < 6 ; i ++ ) { x [ i ] = 0.0 ; for ( aoffset = 0 ;
aoffset < 6 ; aoffset ++ ) { x [ i ] += H [ 6 * aoffset + i ] * grad -> data
[ aoffset ] ; } } * err = fdzsuttcyn1 ( x ) ; * iter = ( real_T ) g_idx_0 +
1.0 ; exitg2 = 1 ; } else { if ( ( A -> size [ 0 ] == 0 ) || ( A -> size [ 1
] == 0 ) ) { aoffset = alpha -> size [ 0 ] ; alpha -> size [ 0 ] = 1 ;
miyl1rq4uj ( alpha , aoffset ) ; alpha -> data [ 0 ] = 0.0 ; } else { aoffset
= A_p -> size [ 0 ] * A_p -> size [ 1 ] ; A_p -> size [ 0 ] = A -> size [ 1 ]
; A_p -> size [ 1 ] = A -> size [ 0 ] ; miyl1rq4uj ( A_p , aoffset ) ; inner
= A -> size [ 0 ] ; loop_ub = A -> size [ 1 ] ; for ( i = 0 ; i < inner ; i
++ ) { for ( aoffset = 0 ; aoffset < loop_ub ; aoffset ++ ) { A_p -> data [
aoffset + A_p -> size [ 0 ] * i ] = A -> data [ A -> size [ 0 ] * aoffset + i
] ; } } l5ks21uytpaq2 ( A , A , tmp_c ) ; cp5ozjiqhu ( tmp_c , A_p , AIn ) ;
i = AIn -> size [ 0 ] - 1 ; inner = AIn -> size [ 1 ] - 1 ; aoffset = alpha
-> size [ 0 ] ; alpha -> size [ 0 ] = AIn -> size [ 0 ] ; miyl1rq4uj ( alpha
, aoffset ) ; if ( i >= 0 ) { memset ( & alpha -> data [ 0 ] , 0 , ( uint32_T
) ( i + 1 ) * sizeof ( real_T ) ) ; } for ( b_k = 0 ; b_k <= inner ; b_k ++ )
{ aoffset = b_k * AIn -> size [ 0 ] - 1 ; scalarLB = ( ( i + 1 ) / 2 ) << 1 ;
vectorUB = scalarLB - 2 ; for ( nx = 0 ; nx <= vectorUB ; nx += 2 ) { tmp_i =
_mm_loadu_pd ( & AIn -> data [ ( aoffset + nx ) + 1 ] ) ; tmp_e =
_mm_loadu_pd ( & alpha -> data [ nx ] ) ; _mm_storeu_pd ( & alpha -> data [
nx ] , _mm_add_pd ( _mm_mul_pd ( tmp_i , _mm_set1_pd ( grad -> data [ b_k ] )
) , tmp_e ) ) ; } for ( nx = scalarLB ; nx <= i ; nx ++ ) { alpha -> data [
nx ] += AIn -> data [ ( aoffset + nx ) + 1 ] * grad -> data [ b_k ] ; } } }
for ( i = 0 ; i < 6 ; i ++ ) { Hg [ i ] = 0.0 ; for ( aoffset = 0 ; aoffset <
6 ; aoffset ++ ) { Hg [ i ] += H [ 6 * aoffset + i ] * grad -> data [ aoffset
] ; } } if ( aofopy35eo ( obj , Hg , alpha ) ) { * exitFlag =
robotics_core_internal_NLPSolverExitFlags_LocalMinimumFound ; b = obj ->
ExtraArgs ; for ( i = 0 ; i < 36 ; i ++ ) { H [ i ] = b -> WeightMatrix [ i ]
; } aoffset = grad -> size [ 0 ] ; grad -> size [ 0 ] = b -> ErrTemp -> size
[ 0 ] ; miyl1rq4uj ( grad , aoffset ) ; inner = b -> ErrTemp -> size [ 0 ] ;
for ( i = 0 ; i < inner ; i ++ ) { grad -> data [ i ] = b -> ErrTemp -> data
[ i ] ; } for ( i = 0 ; i < 6 ; i ++ ) { x [ i ] = 0.0 ; for ( aoffset = 0 ;
aoffset < 6 ; aoffset ++ ) { x [ i ] += H [ 6 * aoffset + i ] * grad -> data
[ aoffset ] ; } } * err = fdzsuttcyn1 ( x ) ; * iter = ( real_T ) g_idx_0 +
1.0 ; exitg2 = 1 ; } else { guard1 = false ; guard2 = false ; guard3 = false
; if ( obj -> ConstraintsOn && ( ( A -> size [ 0 ] != 0 ) && ( A -> size [ 1
] != 0 ) ) ) { l5ks21uytpaq2 ( A , A , tmp_j ) ; bqduk5sgwd ( tmp_j , tmp_f )
; gka31aeazw ( tmp_f , L ) ; nx = L -> size [ 0 ] - 1 ; scalarLB = ( L ->
size [ 0 ] / 2 ) << 1 ; vectorUB = scalarLB - 2 ; for ( b_k = 0 ; b_k <=
vectorUB ; b_k += 2 ) { tmp_i = _mm_loadu_pd ( & L -> data [ b_k ] ) ;
_mm_storeu_pd ( & L -> data [ b_k ] , _mm_sqrt_pd ( tmp_i ) ) ; } for ( b_k =
scalarLB ; b_k <= nx ; b_k ++ ) { L -> data [ b_k ] = muDoubleScalarSqrt ( L
-> data [ b_k ] ) ; } if ( alpha -> size [ 0 ] == L -> size [ 0 ] ) { aoffset
= alpha_p -> size [ 0 ] ; alpha_p -> size [ 0 ] = alpha -> size [ 0 ] ;
miyl1rq4uj ( alpha_p , aoffset ) ; inner = alpha -> size [ 0 ] ; scalarLB = (
alpha -> size [ 0 ] / 2 ) << 1 ; vectorUB = scalarLB - 2 ; for ( i = 0 ; i <=
vectorUB ; i += 2 ) { tmp_i = _mm_loadu_pd ( & alpha -> data [ i ] ) ; tmp_e
= _mm_loadu_pd ( & L -> data [ i ] ) ; _mm_storeu_pd ( & alpha_p -> data [ i
] , _mm_div_pd ( tmp_i , tmp_e ) ) ; } for ( i = scalarLB ; i < inner ; i ++
) { alpha_p -> data [ i ] = alpha -> data [ i ] / L -> data [ i ] ; }
dmv3200pfy ( alpha_p , & costNew , & nx ) ; } else { offa2br1khoeg ( alpha ,
L , & costNew , & nx ) ; } if ( fdzsuttcyn1 ( Hg ) < 0.5 * costNew ) {
mfmaegtoxtz ( activeSet , bb ) ; aoffset = alpha -> size [ 0 ] ; alpha ->
size [ 0 ] = bb -> size [ 0 ] ; miyl1rq4uj ( alpha , aoffset ) ; inner = bb
-> size [ 0 ] ; for ( i = 0 ; i < inner ; i ++ ) { alpha -> data [ i ] = bb
-> data [ i ] ; } activeSet -> data [ ( int32_T ) alpha -> data [ nx - 1 ] -
1 ] = false ; inner = activeSet -> size [ 0 ] - 1 ; i = 0 ; for ( b_k = 0 ;
b_k <= inner ; b_k ++ ) { if ( activeSet -> data [ b_k ] ) { i ++ ; } }
aoffset = eb -> size [ 0 ] ; eb -> size [ 0 ] = i ; os5bv5kg0d ( eb , aoffset
) ; i = 0 ; for ( b_k = 0 ; b_k <= inner ; b_k ++ ) { if ( activeSet -> data
[ b_k ] ) { eb -> data [ i ] = b_k + 1 ; i ++ ; } } aoffset = A -> size [ 0 ]
* A -> size [ 1 ] ; A -> size [ 0 ] = obj -> ConstraintMatrix -> size [ 0 ] ;
A -> size [ 1 ] = eb -> size [ 0 ] ; miyl1rq4uj ( A , aoffset ) ; inner = eb
-> size [ 0 ] ; for ( i = 0 ; i < inner ; i ++ ) { loop_ub = obj ->
ConstraintMatrix -> size [ 0 ] ; for ( aoffset = 0 ; aoffset < loop_ub ;
aoffset ++ ) { A -> data [ aoffset + A -> size [ 0 ] * i ] = obj ->
ConstraintMatrix -> data [ ( eb -> data [ i ] - 1 ) * obj -> ConstraintMatrix
-> size [ 0 ] + aoffset ] ; } } memset ( & P_p [ 0 ] , 0 , 36U * sizeof (
real_T ) ) ; for ( b_k = 0 ; b_k < 6 ; b_k ++ ) { P_p [ b_k + 6 * b_k ] = 1.0
; } aoffset = A_p -> size [ 0 ] * A_p -> size [ 1 ] ; A_p -> size [ 0 ] = A
-> size [ 1 ] ; A_p -> size [ 1 ] = A -> size [ 0 ] ; miyl1rq4uj ( A_p ,
aoffset ) ; inner = A -> size [ 0 ] ; for ( i = 0 ; i < inner ; i ++ ) {
loop_ub = A -> size [ 1 ] ; for ( aoffset = 0 ; aoffset < loop_ub ; aoffset
++ ) { A_p -> data [ aoffset + A_p -> size [ 0 ] * i ] = A -> data [ A ->
size [ 0 ] * aoffset + i ] ; } } l5ks21uytpaq2 ( A , A , tmp_k ) ; cp5ozjiqhu
( tmp_k , A_p , tmp_b ) ; l5ks21uytpaq2z ( A , tmp_b , AIn ) ; if ( ( AIn ->
size [ 0 ] == 6 ) && ( AIn -> size [ 1 ] == 6 ) ) { for ( i = 0 ; i <= 34 ; i
+= 2 ) { tmp_i = _mm_loadu_pd ( & P_p [ i ] ) ; tmp_e = _mm_loadu_pd ( & AIn
-> data [ i ] ) ; _mm_storeu_pd ( & P_p [ i ] , _mm_sub_pd ( tmp_i , tmp_e )
) ; } } else { bsiqjn4ttti ( P_p , AIn ) ; } b_k = ( int32_T ) alpha -> data
[ nx - 1 ] - 1 ; aoffset = alpha -> size [ 0 ] ; alpha -> size [ 0 ] = obj ->
ConstraintMatrix -> size [ 0 ] ; miyl1rq4uj ( alpha , aoffset ) ; inner = obj
-> ConstraintMatrix -> size [ 0 ] ; for ( i = 0 ; i < inner ; i ++ ) { alpha
-> data [ i ] = obj -> ConstraintMatrix -> data [ obj -> ConstraintMatrix ->
size [ 0 ] * b_k + i ] ; } costNew = 0.0 ; for ( i = 0 ; i < 6 ; i ++ ) { Hg
[ i ] = 0.0 ; for ( aoffset = 0 ; aoffset < 6 ; aoffset ++ ) { Hg [ i ] +=
P_p [ 6 * i + aoffset ] * alpha -> data [ aoffset ] ; } costNew += Hg [ i ] *
alpha -> data [ i ] ; } idxl = 1.0 / costNew ; for ( i = 0 ; i <= 34 ; i += 2
) { tmp_i = _mm_loadu_pd ( & P_p [ i ] ) ; _mm_storeu_pd ( & idxl_p [ i ] ,
_mm_mul_pd ( _mm_set1_pd ( idxl ) , tmp_i ) ) ; } aoffset = A_p -> size [ 0 ]
* A_p -> size [ 1 ] ; A_p -> size [ 0 ] = alpha -> size [ 0 ] ; A_p -> size [
1 ] = alpha -> size [ 0 ] ; miyl1rq4uj ( A_p , aoffset ) ; inner = alpha ->
size [ 0 ] ; for ( i = 0 ; i < inner ; i ++ ) { loop_ub = alpha -> size [ 0 ]
; scalarLB = ( alpha -> size [ 0 ] / 2 ) << 1 ; vectorUB = scalarLB - 2 ; for
( aoffset = 0 ; aoffset <= vectorUB ; aoffset += 2 ) { tmp_i = _mm_loadu_pd (
& alpha -> data [ aoffset ] ) ; _mm_storeu_pd ( & A_p -> data [ aoffset + A_p
-> size [ 0 ] * i ] , _mm_mul_pd ( tmp_i , _mm_set1_pd ( alpha -> data [ i ]
) ) ) ; } for ( aoffset = scalarLB ; aoffset < loop_ub ; aoffset ++ ) { A_p
-> data [ aoffset + A_p -> size [ 0 ] * i ] = alpha -> data [ aoffset ] *
alpha -> data [ i ] ; } } l5ks21uytpaq ( idxl_p , A_p , a__2 ) ; for ( i = 0
; i < 6 ; i ++ ) { for ( aoffset = 0 ; aoffset < 6 ; aoffset ++ ) { idxl =
0.0 ; for ( inner = 0 ; inner < 6 ; inner ++ ) { idxl += a__2 -> data [ 6 *
inner + i ] * P_p [ 6 * aoffset + inner ] ; } H [ i + 6 * aoffset ] += idxl ;
} } g_idx_0 ++ ; } else { guard3 = true ; } } else { guard3 = true ; } if (
guard3 ) { for ( i = 0 ; i <= 4 ; i += 2 ) { tmp_i = _mm_loadu_pd ( & Hg [ i
] ) ; _mm_storeu_pd ( & Hg [ i ] , _mm_mul_pd ( tmp_i , _mm_set1_pd ( - 1.0 )
) ) ; } idxl = - 1.0 ; if ( obj -> ConstraintsOn ) { aoffset = activeSet_p ->
size [ 0 ] ; activeSet_p -> size [ 0 ] = activeSet -> size [ 0 ] ; dw1xs30knw
( activeSet_p , aoffset ) ; inner = activeSet -> size [ 0 ] ; for ( i = 0 ; i
< inner ; i ++ ) { activeSet_p -> data [ i ] = ! activeSet -> data [ i ] ; }
if ( pahyhnfmn2 ( activeSet_p ) ) { nx = activeSet -> size [ 0 ] - 1 ; inner
= 0 ; for ( b_k = 0 ; b_k <= nx ; b_k ++ ) { if ( ! activeSet -> data [ b_k ]
) { inner ++ ; } } aoffset = cb -> size [ 0 ] ; cb -> size [ 0 ] = inner ;
os5bv5kg0d ( cb , aoffset ) ; inner = 0 ; for ( b_k = 0 ; b_k <= nx ; b_k ++
) { if ( ! activeSet -> data [ b_k ] ) { cb -> data [ inner ] = b_k + 1 ;
inner ++ ; } } aoffset = alpha -> size [ 0 ] ; alpha -> size [ 0 ] = cb ->
size [ 0 ] ; miyl1rq4uj ( alpha , aoffset ) ; inner = cb -> size [ 0 ] ; for
( i = 0 ; i < inner ; i ++ ) { alpha -> data [ i ] = obj -> ConstraintBound
-> data [ cb -> data [ i ] - 1 ] ; } inner = 0 ; for ( b_k = 0 ; b_k <= nx ;
b_k ++ ) { if ( ! activeSet -> data [ b_k ] ) { inner ++ ; } } aoffset = db
-> size [ 0 ] ; db -> size [ 0 ] = inner ; os5bv5kg0d ( db , aoffset ) ;
inner = 0 ; for ( b_k = 0 ; b_k <= nx ; b_k ++ ) { if ( ! activeSet -> data [
b_k ] ) { db -> data [ inner ] = b_k + 1 ; inner ++ ; } } aoffset = AIn ->
size [ 0 ] * AIn -> size [ 1 ] ; AIn -> size [ 0 ] = obj -> ConstraintMatrix
-> size [ 0 ] ; AIn -> size [ 1 ] = db -> size [ 0 ] ; miyl1rq4uj ( AIn ,
aoffset ) ; inner = db -> size [ 0 ] ; for ( i = 0 ; i < inner ; i ++ ) {
loop_ub = obj -> ConstraintMatrix -> size [ 0 ] ; for ( aoffset = 0 ; aoffset
< loop_ub ; aoffset ++ ) { AIn -> data [ aoffset + AIn -> size [ 0 ] * i ] =
obj -> ConstraintMatrix -> data [ ( db -> data [ i ] - 1 ) * obj ->
ConstraintMatrix -> size [ 0 ] + aoffset ] ; } } l5ks21uytpa ( AIn , x , L )
; l5ks21uytpa ( AIn , Hg , alpha_p ) ; if ( ( alpha -> size [ 0 ] == L ->
size [ 0 ] ) && ( ( alpha -> size [ 0 ] == 1 ? L -> size [ 0 ] : alpha ->
size [ 0 ] ) == alpha_p -> size [ 0 ] ) ) { inner = alpha -> size [ 0 ] ;
scalarLB = ( alpha -> size [ 0 ] / 2 ) << 1 ; vectorUB = scalarLB - 2 ; for (
i = 0 ; i <= vectorUB ; i += 2 ) { tmp_i = _mm_loadu_pd ( & alpha -> data [ i
] ) ; tmp_e = _mm_loadu_pd ( & L -> data [ i ] ) ; tmp_p = _mm_loadu_pd ( &
alpha_p -> data [ i ] ) ; _mm_storeu_pd ( & alpha -> data [ i ] , _mm_div_pd
( _mm_sub_pd ( tmp_i , tmp_e ) , tmp_p ) ) ; } for ( i = scalarLB ; i < inner
; i ++ ) { alpha -> data [ i ] = ( alpha -> data [ i ] - L -> data [ i ] ) /
alpha_p -> data [ i ] ; } } else { offa2br1khoe ( alpha , L , alpha_p ) ; }
aoffset = activeSet_p -> size [ 0 ] ; activeSet_p -> size [ 0 ] = alpha ->
size [ 0 ] ; dw1xs30knw ( activeSet_p , aoffset ) ; inner = alpha -> size [ 0
] ; for ( i = 0 ; i < inner ; i ++ ) { activeSet_p -> data [ i ] = ( alpha ->
data [ i ] > 0.0 ) ; } mfmaegtoxtz ( activeSet_p , bb ) ; aoffset = L -> size
[ 0 ] ; L -> size [ 0 ] = bb -> size [ 0 ] ; miyl1rq4uj ( L , aoffset ) ;
inner = bb -> size [ 0 ] ; for ( i = 0 ; i < inner ; i ++ ) { L -> data [ i ]
= bb -> data [ i ] ; } if ( L -> size [ 0 ] != 0 ) { nx = alpha -> size [ 0 ]
- 1 ; inner = 0 ; for ( b_k = 0 ; b_k <= nx ; b_k ++ ) { if ( alpha -> data [
b_k ] > 0.0 ) { inner ++ ; } } aoffset = fb -> size [ 0 ] ; fb -> size [ 0 ]
= inner ; os5bv5kg0d ( fb , aoffset ) ; inner = 0 ; for ( b_k = 0 ; b_k <= nx
; b_k ++ ) { if ( alpha -> data [ b_k ] > 0.0 ) { fb -> data [ inner ] = b_k
+ 1 ; inner ++ ; } } aoffset = alpha_p -> size [ 0 ] ; alpha_p -> size [ 0 ]
= fb -> size [ 0 ] ; miyl1rq4uj ( alpha_p , aoffset ) ; inner = fb -> size [
0 ] ; for ( i = 0 ; i < inner ; i ++ ) { alpha_p -> data [ i ] = alpha ->
data [ fb -> data [ i ] - 1 ] ; } heltobv2ai ( alpha_p , & lambda , & nx ) ;
aoffset = activeSet_p -> size [ 0 ] ; activeSet_p -> size [ 0 ] = activeSet
-> size [ 0 ] ; dw1xs30knw ( activeSet_p , aoffset ) ; inner = activeSet ->
size [ 0 ] ; for ( i = 0 ; i < inner ; i ++ ) { activeSet_p -> data [ i ] = !
activeSet -> data [ i ] ; } mfmaegtoxtz ( activeSet_p , tmp_g ) ; idxl =
tmp_g -> data [ ( int32_T ) L -> data [ nx - 1 ] - 1 ] ; } else { lambda =
0.0 ; } } else { lambda = 0.0 ; } } else { lambda = 0.0 ; } if ( lambda > 0.0
) { b_gamma = muDoubleScalarMin ( 1.0 , lambda ) ; } else { b_gamma = 1.0 ; }
beta = obj -> ArmijoRuleBeta ; sigma = obj -> ArmijoRuleSigma ; for ( i = 0 ;
i <= 4 ; i += 2 ) { tmp_i = _mm_loadu_pd ( & Hg [ i ] ) ; tmp_e =
_mm_loadu_pd ( & x [ i ] ) ; _mm_storeu_pd ( & sNew [ i ] , _mm_add_pd (
_mm_mul_pd ( _mm_set1_pd ( b_gamma ) , tmp_i ) , tmp_e ) ) ; } pbvdpfzges (
sNew , obj -> ExtraArgs , & costNew , P_p , a__2 , & b ) ; obj -> ExtraArgs =
b ; m = 0.0 ; do { exitg1 = 0 ; sigma_p = 0.0 ; for ( i = 0 ; i < 6 ; i ++ )
{ sigma_p += - sigma * grad -> data [ i ] * ( b_gamma * Hg [ i ] ) ; } if (
cost - costNew < sigma_p ) { flag = ( b_gamma < obj -> StepTolerance ) ; if (
flag ) { * exitFlag =
robotics_core_internal_NLPSolverExitFlags_StepSizeBelowMinimum ; b = obj ->
ExtraArgs ; for ( i = 0 ; i < 36 ; i ++ ) { H [ i ] = b -> WeightMatrix [ i ]
; } aoffset = grad -> size [ 0 ] ; grad -> size [ 0 ] = b -> ErrTemp -> size
[ 0 ] ; miyl1rq4uj ( grad , aoffset ) ; inner = b -> ErrTemp -> size [ 0 ] ;
for ( i = 0 ; i < inner ; i ++ ) { grad -> data [ i ] = b -> ErrTemp -> data
[ i ] ; } for ( i = 0 ; i < 6 ; i ++ ) { x [ i ] = 0.0 ; for ( aoffset = 0 ;
aoffset < 6 ; aoffset ++ ) { x [ i ] += H [ 6 * aoffset + i ] * grad -> data
[ aoffset ] ; } } * err = fdzsuttcyn1 ( x ) ; * iter = ( real_T ) g_idx_0 +
1.0 ; exitg1 = 1 ; } else { b_gamma *= beta ; m ++ ; for ( i = 0 ; i <= 4 ; i
+= 2 ) { tmp_i = _mm_loadu_pd ( & Hg [ i ] ) ; tmp_e = _mm_loadu_pd ( & x [ i
] ) ; _mm_storeu_pd ( & sNew [ i ] , _mm_add_pd ( _mm_mul_pd ( _mm_set1_pd (
b_gamma ) , tmp_i ) , tmp_e ) ) ; } pbvdpfzges ( sNew , obj -> ExtraArgs , &
costNew , P_p , a__2 , & b ) ; obj -> ExtraArgs = b ; } } else { for ( i = 0
; i <= 4 ; i += 2 ) { tmp_i = _mm_loadu_pd ( & Hg [ i ] ) ; tmp_e =
_mm_loadu_pd ( & x [ i ] ) ; _mm_storeu_pd ( & xSol [ i ] , _mm_add_pd (
_mm_mul_pd ( _mm_set1_pd ( b_gamma ) , tmp_i ) , tmp_e ) ) ; } b = obj ->
ExtraArgs ; aoffset = alpha -> size [ 0 ] ; alpha -> size [ 0 ] = b ->
GradTemp -> size [ 0 ] ; miyl1rq4uj ( alpha , aoffset ) ; inner = b ->
GradTemp -> size [ 0 ] ; for ( i = 0 ; i < inner ; i ++ ) { alpha -> data [ i
] = b -> GradTemp -> data [ i ] ; } exitg1 = 2 ; } } while ( exitg1 == 0 ) ;
if ( exitg1 == 1 ) { exitg2 = 1 ; } else if ( ( m == 0.0 ) && (
muDoubleScalarAbs ( b_gamma - lambda ) < 1.4901161193847656E-8 ) ) { b_k = (
int32_T ) idxl ; aoffset = grad -> size [ 0 ] ; grad -> size [ 0 ] = obj ->
ConstraintMatrix -> size [ 0 ] ; miyl1rq4uj ( grad , aoffset ) ; inner = obj
-> ConstraintMatrix -> size [ 0 ] ; for ( i = 0 ; i < inner ; i ++ ) { grad
-> data [ i ] = obj -> ConstraintMatrix -> data [ ( b_k - 1 ) * obj ->
ConstraintMatrix -> size [ 0 ] + i ] ; } activeSet -> data [ ( int32_T ) idxl
- 1 ] = true ; nx = activeSet -> size [ 0 ] - 1 ; inner = 0 ; for ( b_k = 0 ;
b_k <= nx ; b_k ++ ) { if ( activeSet -> data [ b_k ] ) { inner ++ ; } }
aoffset = gb -> size [ 0 ] ; gb -> size [ 0 ] = inner ; os5bv5kg0d ( gb ,
aoffset ) ; inner = 0 ; for ( b_k = 0 ; b_k <= nx ; b_k ++ ) { if ( activeSet
-> data [ b_k ] ) { gb -> data [ inner ] = b_k + 1 ; inner ++ ; } } aoffset =
A -> size [ 0 ] * A -> size [ 1 ] ; A -> size [ 0 ] = obj -> ConstraintMatrix
-> size [ 0 ] ; A -> size [ 1 ] = gb -> size [ 0 ] ; miyl1rq4uj ( A , aoffset
) ; inner = gb -> size [ 0 ] ; for ( i = 0 ; i < inner ; i ++ ) { loop_ub =
obj -> ConstraintMatrix -> size [ 0 ] ; for ( aoffset = 0 ; aoffset < loop_ub
; aoffset ++ ) { A -> data [ aoffset + A -> size [ 0 ] * i ] = obj ->
ConstraintMatrix -> data [ ( gb -> data [ i ] - 1 ) * obj -> ConstraintMatrix
-> size [ 0 ] + aoffset ] ; } } lambda = 0.0 ; for ( i = 0 ; i < 6 ; i ++ ) {
grad_p [ i ] = 0.0 ; for ( aoffset = 0 ; aoffset < 6 ; aoffset ++ ) { grad_p
[ i ] += H [ 6 * i + aoffset ] * grad -> data [ aoffset ] ; } lambda +=
grad_p [ i ] * grad -> data [ i ] ; } idxl = 1.0 / lambda ; aoffset = A_p ->
size [ 0 ] * A_p -> size [ 1 ] ; A_p -> size [ 0 ] = grad -> size [ 0 ] ; A_p
-> size [ 1 ] = grad -> size [ 0 ] ; miyl1rq4uj ( A_p , aoffset ) ; inner =
grad -> size [ 0 ] ; for ( i = 0 ; i < inner ; i ++ ) { loop_ub = grad ->
size [ 0 ] ; scalarLB = ( grad -> size [ 0 ] / 2 ) << 1 ; vectorUB = scalarLB
- 2 ; for ( aoffset = 0 ; aoffset <= vectorUB ; aoffset += 2 ) { tmp_i =
_mm_loadu_pd ( & grad -> data [ aoffset ] ) ; _mm_storeu_pd ( & A_p -> data [
aoffset + A_p -> size [ 0 ] * i ] , _mm_mul_pd ( tmp_i , _mm_set1_pd ( grad
-> data [ i ] ) ) ) ; } for ( aoffset = scalarLB ; aoffset < loop_ub ;
aoffset ++ ) { A_p -> data [ aoffset + A_p -> size [ 0 ] * i ] = grad -> data
[ aoffset ] * grad -> data [ i ] ; } } l5ks21uytpaq2zf ( A_p , H , tmp ) ;
for ( i = 0 ; i < 6 ; i ++ ) { for ( aoffset = 0 ; aoffset < 6 ; aoffset ++ )
{ H_p [ i + 6 * aoffset ] = 0.0 ; for ( inner = 0 ; inner < 6 ; inner ++ ) {
H_p [ i + 6 * aoffset ] += H [ 6 * inner + i ] * tmp -> data [ 6 * aoffset +
inner ] ; } } } for ( i = 0 ; i <= 34 ; i += 2 ) { tmp_i = _mm_loadu_pd ( &
H_p [ i ] ) ; tmp_e = _mm_loadu_pd ( & H [ i ] ) ; _mm_storeu_pd ( & H [ i ]
, _mm_sub_pd ( tmp_e , _mm_mul_pd ( _mm_set1_pd ( idxl ) , tmp_i ) ) ) ; }
guard2 = true ; } else { if ( alpha -> size [ 0 ] == grad -> size [ 0 ] ) {
aoffset = grad -> size [ 0 ] ; grad -> size [ 0 ] = alpha -> size [ 0 ] ;
miyl1rq4uj ( grad , aoffset ) ; inner = alpha -> size [ 0 ] ; scalarLB = (
alpha -> size [ 0 ] / 2 ) << 1 ; vectorUB = scalarLB - 2 ; for ( i = 0 ; i <=
vectorUB ; i += 2 ) { tmp_i = _mm_loadu_pd ( & alpha -> data [ i ] ) ; tmp_e
= _mm_loadu_pd ( & grad -> data [ i ] ) ; _mm_storeu_pd ( & grad -> data [ i
] , _mm_sub_pd ( tmp_i , tmp_e ) ) ; } for ( i = scalarLB ; i < inner ; i ++
) { grad -> data [ i ] = alpha -> data [ i ] - grad -> data [ i ] ; } } else
{ bsiqjn4ttti1 ( grad , alpha ) ; } cost = 0.0 ; idxl = 0.0 ; for ( i = 0 ; i
< 6 ; i ++ ) { cost += Hg [ i ] * grad -> data [ i ] ; sNew [ i ] = 0.0 ; for
( aoffset = 0 ; aoffset < 6 ; aoffset ++ ) { sNew [ i ] += H [ 6 * i +
aoffset ] * ( 0.2 * grad -> data [ aoffset ] ) ; } idxl += sNew [ i ] * grad
-> data [ i ] ; } if ( cost < idxl ) { idxl = 0.0 ; lambda = 0.0 ; cost = 0.0
; for ( i = 0 ; i < 6 ; i ++ ) { sNew [ i ] = 0.0 ; grad_p [ i ] = 0.0 ; for
( aoffset = 0 ; aoffset < 6 ; aoffset ++ ) { b_gamma = grad -> data [ aoffset
] ; sNew [ i ] += H [ 6 * i + aoffset ] * ( 0.8 * b_gamma ) ; grad_p [ i ] +=
H [ 6 * i + aoffset ] * b_gamma ; } idxl += sNew [ i ] * grad -> data [ i ] ;
lambda += grad_p [ i ] * grad -> data [ i ] ; cost += Hg [ i ] * grad -> data
[ i ] ; } cost = idxl / ( lambda - cost ) ; } else { cost = 1.0 ; } lambda =
0.0 ; for ( i = 0 ; i < 6 ; i ++ ) { idxl = 0.0 ; for ( aoffset = 0 ; aoffset
< 6 ; aoffset ++ ) { idxl += H [ 6 * aoffset + i ] * ( 1.0 - cost ) * grad ->
data [ aoffset ] ; } idxl += cost * Hg [ i ] ; lambda += idxl * grad -> data
[ i ] ; sNew [ i ] = idxl ; } memset ( & P_p [ 0 ] , 0 , 36U * sizeof (
real_T ) ) ; for ( b_k = 0 ; b_k < 6 ; b_k ++ ) { P_p [ b_k + 6 * b_k ] = 1.0
; } if ( grad -> size [ 0 ] == 6 ) { for ( i = 0 ; i < 6 ; i ++ ) { for (
aoffset = 0 ; aoffset <= 4 ; aoffset += 2 ) { tmp_i = _mm_loadu_pd ( & sNew [
aoffset ] ) ; tmp_e = _mm_loadu_pd ( & P_p [ 6 * i + aoffset ] ) ;
_mm_storeu_pd ( & V [ aoffset + 6 * i ] , _mm_sub_pd ( tmp_e , _mm_div_pd (
_mm_mul_pd ( tmp_i , _mm_set1_pd ( grad -> data [ i ] ) ) , _mm_set1_pd (
lambda ) ) ) ) ; } } } else { offa2br1kho ( V , P_p , sNew , grad , lambda )
; } for ( i = 0 ; i < 6 ; i ++ ) { for ( aoffset = 0 ; aoffset < 6 ; aoffset
++ ) { H_p [ i + 6 * aoffset ] = 0.0 ; for ( inner = 0 ; inner < 6 ; inner ++
) { H_p [ i + 6 * aoffset ] += V [ 6 * inner + i ] * H [ 6 * aoffset + inner
] ; } } for ( aoffset = 0 ; aoffset < 6 ; aoffset ++ ) { idxl_p [ i + 6 *
aoffset ] = 0.0 ; for ( inner = 0 ; inner < 6 ; inner ++ ) { idxl_p [ i + 6 *
aoffset ] += H_p [ 6 * inner + i ] * V [ 6 * inner + aoffset ] ; } } for (
aoffset = 0 ; aoffset <= 4 ; aoffset += 2 ) { tmp_i = _mm_loadu_pd ( & sNew [
aoffset ] ) ; _mm_storeu_pd ( & P_p [ aoffset + 6 * i ] , _mm_div_pd (
_mm_mul_pd ( tmp_i , _mm_set1_pd ( sNew [ i ] ) ) , _mm_set1_pd ( lambda ) )
) ; } } for ( i = 0 ; i < 36 ; i ++ ) { cost = idxl_p [ i ] + P_p [ i ] ; H_p
[ i ] = 1.4901161193847656E-8 * ( real_T ) tmp_m [ i ] + cost ; H [ i ] =
cost ; } if ( ! np5inobdqs ( H_p ) ) { * exitFlag =
robotics_core_internal_NLPSolverExitFlags_HessianNotPositiveSemidefinite ; b
= obj -> ExtraArgs ; for ( i = 0 ; i < 36 ; i ++ ) { H [ i ] = b ->
WeightMatrix [ i ] ; } aoffset = grad -> size [ 0 ] ; grad -> size [ 0 ] = b
-> ErrTemp -> size [ 0 ] ; miyl1rq4uj ( grad , aoffset ) ; inner = b ->
ErrTemp -> size [ 0 ] ; for ( i = 0 ; i < inner ; i ++ ) { grad -> data [ i ]
= b -> ErrTemp -> data [ i ] ; } for ( i = 0 ; i < 6 ; i ++ ) { x [ i ] = 0.0
; for ( aoffset = 0 ; aoffset < 6 ; aoffset ++ ) { x [ i ] += H [ 6 * aoffset
+ i ] * grad -> data [ aoffset ] ; } } * err = fdzsuttcyn1 ( x ) ; * iter = (
real_T ) g_idx_0 + 1.0 ; exitg2 = 1 ; } else { guard2 = true ; } } } if (
guard2 ) { if ( obj -> ConstraintsOn ) { aoffset = AIn -> size [ 0 ] * AIn ->
size [ 1 ] ; AIn -> size [ 0 ] = obj -> ConstraintMatrix -> size [ 0 ] ; AIn
-> size [ 1 ] = obj -> ConstraintMatrix -> size [ 1 ] ; miyl1rq4uj ( AIn ,
aoffset ) ; inner = obj -> ConstraintMatrix -> size [ 0 ] * obj ->
ConstraintMatrix -> size [ 1 ] ; for ( i = 0 ; i < inner ; i ++ ) { AIn ->
data [ i ] = obj -> ConstraintMatrix -> data [ i ] ; } l5ks21uytpa ( AIn ,
xSol , L ) ; aoffset = activeSet_p -> size [ 0 ] ; activeSet_p -> size [ 0 ]
= obj -> ConstraintBound -> size [ 0 ] == 1 ? L -> size [ 0 ] : obj ->
ConstraintBound -> size [ 0 ] ; dw1xs30knw ( activeSet_p , aoffset ) ; b_k =
( L -> size [ 0 ] != 1 ) ; nx = ( obj -> ConstraintBound -> size [ 0 ] != 1 )
; inner = obj -> ConstraintBound -> size [ 0 ] == 1 ? L -> size [ 0 ] : obj
-> ConstraintBound -> size [ 0 ] ; for ( i = 0 ; i < inner ; i ++ ) {
activeSet_p -> data [ i ] = ( L -> data [ i * b_k ] - obj -> ConstraintBound
-> data [ i * nx ] > 1.4901161193847656E-8 ) ; } if ( pahyhnfmn2 (
activeSet_p ) ) { for ( i = 0 ; i < 6 ; i ++ ) { xSol [ i ] = x [ i ] ; } *
exitFlag = robotics_core_internal_NLPSolverExitFlags_SearchDirectionInvalid ;
b = obj -> ExtraArgs ; for ( i = 0 ; i < 36 ; i ++ ) { H [ i ] = b ->
WeightMatrix [ i ] ; } aoffset = grad -> size [ 0 ] ; grad -> size [ 0 ] = b
-> ErrTemp -> size [ 0 ] ; miyl1rq4uj ( grad , aoffset ) ; inner = b ->
ErrTemp -> size [ 0 ] ; for ( i = 0 ; i < inner ; i ++ ) { grad -> data [ i ]
= b -> ErrTemp -> data [ i ] ; } for ( i = 0 ; i < 6 ; i ++ ) { x [ i ] = 0.0
; for ( aoffset = 0 ; aoffset < 6 ; aoffset ++ ) { x [ i ] += H [ 6 * aoffset
+ i ] * grad -> data [ aoffset ] ; } } * err = fdzsuttcyn1 ( x ) ; * iter = (
real_T ) g_idx_0 + 1.0 ; exitg2 = 1 ; } else { guard1 = true ; } } else {
guard1 = true ; } } if ( guard1 ) { for ( i = 0 ; i < 6 ; i ++ ) { x [ i ] =
xSol [ i ] ; } aoffset = grad -> size [ 0 ] ; grad -> size [ 0 ] = alpha ->
size [ 0 ] ; miyl1rq4uj ( grad , aoffset ) ; inner = alpha -> size [ 0 ] ; if
( inner - 1 >= 0 ) { memcpy ( & grad -> data [ 0 ] , & alpha -> data [ 0 ] ,
( uint32_T ) inner * sizeof ( real_T ) ) ; } cost = costNew ; g_idx_0 ++ ; }
} } } else { * exitFlag =
robotics_core_internal_NLPSolverExitFlags_IterationLimitExceeded ; b = obj ->
ExtraArgs ; for ( i = 0 ; i < 36 ; i ++ ) { H [ i ] = b -> WeightMatrix [ i ]
; } aoffset = grad -> size [ 0 ] ; grad -> size [ 0 ] = b -> ErrTemp -> size
[ 0 ] ; miyl1rq4uj ( grad , aoffset ) ; inner = b -> ErrTemp -> size [ 0 ] ;
for ( i = 0 ; i < inner ; i ++ ) { grad -> data [ i ] = b -> ErrTemp -> data
[ i ] ; } for ( i = 0 ; i < 6 ; i ++ ) { x [ i ] = 0.0 ; for ( aoffset = 0 ;
aoffset < 6 ; aoffset ++ ) { x [ i ] += H [ 6 * aoffset + i ] * grad -> data
[ aoffset ] ; } } * err = fdzsuttcyn1 ( x ) ; * iter = obj ->
MaxNumIterationInternal ; exitg2 = 1 ; } } while ( exitg2 == 0 ) ; bdhoypqe5x
( & tmp_b ) ; bdhoypqe5x ( & tmp_k ) ; bdhoypqe5x ( & tmp_c ) ; bdhoypqe5x (
& tmp_f ) ; bdhoypqe5x ( & tmp_j ) ; preyilpnl4 ( & tmp_g ) ; bdhoypqe5x ( &
alpha_p ) ; naapodi4lu ( & activeSet_p ) ; bdhoypqe5x ( & A_p ) ; bdhoypqe5x
( & tmp ) ; preyilpnl4 ( & gb ) ; preyilpnl4 ( & fb ) ; preyilpnl4 ( & eb ) ;
preyilpnl4 ( & db ) ; preyilpnl4 ( & cb ) ; preyilpnl4 ( & bb ) ; bdhoypqe5x
( & L ) ; bdhoypqe5x ( & AIn ) ; bdhoypqe5x ( & alpha ) ; bdhoypqe5x ( & A )
; naapodi4lu ( & activeSet ) ; bdhoypqe5x ( & grad ) ; bdhoypqe5x ( & a__2 )
; } static void inyjui0sxx ( const real_T varargin_1 [ 2 ] , klgljlxbmf * r )
{ real_T xi [ 257 ] ; real_T b_r ; real_T d_u ; real_T x ; int32_T b_k ;
int32_T d ; int32_T i ; uint32_T u32 [ 2 ] ; static const real_T tmp [ 257 ]
= { 1.0 , 0.977101701267673 , 0.959879091800108 , 0.9451989534423 ,
0.932060075959231 , 0.919991505039348 , 0.908726440052131 , 0.898095921898344
, 0.887984660755834 , 0.878309655808918 , 0.869008688036857 ,
0.860033621196332 , 0.851346258458678 , 0.842915653112205 , 0.834716292986884
, 0.826726833946222 , 0.818929191603703 , 0.811307874312656 ,
0.803849483170964 , 0.796542330422959 , 0.789376143566025 , 0.782341832654803
, 0.775431304981187 , 0.768637315798486 , 0.761953346836795 ,
0.755373506507096 , 0.748892447219157 , 0.742505296340151 , 0.736207598126863
, 0.729995264561476 , 0.72386453346863 , 0.717811932630722 ,
0.711834248878248 , 0.705928501332754 , 0.700091918136512 , 0.694321916126117
, 0.688616083004672 , 0.682972161644995 , 0.677388036218774 ,
0.671861719897082 , 0.66639134390875 , 0.660975147776663 , 0.655611470579697
, 0.650298743110817 , 0.645035480820822 , 0.639820277453057 ,
0.634651799287624 , 0.629528779924837 , 0.624450015547027 , 0.619414360605834
, 0.614420723888914 , 0.609468064925773 , 0.604555390697468 ,
0.599681752619125 , 0.594846243767987 , 0.590047996332826 , 0.585286179263371
, 0.580559996100791 , 0.575868682972354 , 0.571211506735253 ,
0.566587763256165 , 0.561996775814525 , 0.557437893618766 , 0.552910490425833
, 0.548413963255266 , 0.543947731190026 , 0.539511234256952 ,
0.535103932380458 , 0.530725304403662 , 0.526374847171684 , 0.522052074672322
, 0.517756517229756 , 0.513487720747327 , 0.509245245995748 ,
0.505028667943468 , 0.500837575126149 , 0.49667156905249 , 0.492530263643869
, 0.488413284705458 , 0.484320269426683 , 0.480250865909047 ,
0.476204732719506 , 0.47218153846773 , 0.468180961405694 , 0.464202689048174
, 0.460246417812843 , 0.456311852678716 , 0.452398706861849 ,
0.448506701507203 , 0.444635565395739 , 0.440785034665804 , 0.436954852547985
, 0.433144769112652 , 0.429354541029442 , 0.425583931338022 ,
0.421832709229496 , 0.418100649837848 , 0.414387534040891 , 0.410693148270188
, 0.407017284329473 , 0.403359739221114 , 0.399720314980197 ,
0.396098818515832 , 0.392495061459315 , 0.388908860018789 , 0.385340034840077
, 0.381788410873393 , 0.378253817245619 , 0.374736087137891 ,
0.371235057668239 , 0.367750569779032 , 0.364282468129004 , 0.360830600989648
, 0.357394820145781 , 0.353974980800077 , 0.350570941481406 ,
0.347182563956794 , 0.343809713146851 , 0.340452257044522 , 0.337110066637006
, 0.333783015830718 , 0.330470981379163 , 0.327173842813601 ,
0.323891482376391 , 0.320623784956905 , 0.317370638029914 , 0.314131931596337
, 0.310907558126286 , 0.307697412504292 , 0.30450139197665 ,
0.301319396100803 , 0.298151326696685 , 0.294997087799962 , 0.291856585617095
, 0.288729728482183 , 0.285616426815502 , 0.282516593083708 ,
0.279430141761638 , 0.276356989295668 , 0.273297054068577 , 0.270250256365875
, 0.267216518343561 , 0.264195763997261 , 0.261187919132721 ,
0.258192911337619 , 0.255210669954662 , 0.252241126055942 , 0.249284212418529
, 0.246339863501264 , 0.24340801542275 , 0.240488605940501 ,
0.237581574431238 , 0.23468686187233 , 0.231804410824339 , 0.228934165414681
, 0.226076071322381 , 0.223230075763918 , 0.220396127480152 ,
0.217574176724331 , 0.214764175251174 , 0.211966076307031 , 0.209179834621125
, 0.206405406397881 , 0.203642749310335 , 0.200891822494657 ,
0.198152586545776 , 0.195425003514135 , 0.192709036903589 , 0.190004651670465
, 0.187311814223801 , 0.1846304924268 , 0.181960655599523 , 0.179302274522848
, 0.176655321443735 , 0.174019770081839 , 0.171395595637506 ,
0.168782774801212 , 0.166181285764482 , 0.163591108232366 , 0.161012223437511
, 0.158444614155925 , 0.15588826472448 , 0.153343161060263 ,
0.150809290681846 , 0.148286642732575 , 0.145775208005994 , 0.143274978973514
, 0.140785949814445 , 0.138308116448551 , 0.135841476571254 ,
0.133386029691669 , 0.130941777173644 , 0.12850872228 , 0.126086870220186 ,
0.123676228201597 , 0.12127680548479 , 0.11888861344291 , 0.116511665625611 ,
0.114145977827839 , 0.111791568163838 , 0.109448457146812 , 0.107116667774684
, 0.104796225622487 , 0.102487158941935 , 0.10018949876881 ,
0.0979032790388625 , 0.095628536713009 , 0.093365311912691 ,
0.0911136480663738 , 0.0888735920682759 , 0.0866451944505581 ,
0.0844285095703535 , 0.082223595813203 , 0.0800305158146631 ,
0.0778493367020961 , 0.0756801303589272 , 0.0735229737139814 ,
0.0713779490588905 , 0.0692451443970068 , 0.0671246538277886 ,
0.065016577971243 , 0.0629210244377582 , 0.06083810834954 ,
0.0587679529209339 , 0.0567106901062031 , 0.0546664613248891 ,
0.0526354182767924 , 0.0506177238609479 , 0.0486135532158687 ,
0.0466230949019305 , 0.0446465522512946 , 0.0426841449164746 ,
0.0407361106559411 , 0.0388027074045262 , 0.0368842156885674 ,
0.0349809414617162 , 0.0330932194585786 , 0.0312214171919203 ,
0.0293659397581334 , 0.0275272356696031 , 0.0257058040085489 ,
0.0239022033057959 , 0.0221170627073089 , 0.0203510962300445 ,
0.0186051212757247 , 0.0168800831525432 , 0.0151770883079353 ,
0.0134974506017399 , 0.0118427578579079 , 0.0102149714397015 ,
0.00861658276939875 , 0.00705087547137324 , 0.00552240329925101 ,
0.00403797259336304 , 0.00260907274610216 , 0.0012602859304986 ,
0.000477467764609386 } ; const real_T * fitab ; int32_T exitg1 ; int32_T
exitg2 ; b_k = r -> size [ 0 ] ; r -> size [ 0 ] = ( int32_T ) varargin_1 [ 0
] ; miyl1rq4uj ( r , b_k ) ; d = ( int32_T ) varargin_1 [ 0 ] - 1 ; if ( (
int32_T ) varargin_1 [ 0 ] - 1 >= 0 ) { xi [ 0 ] = 0.0 ; xi [ 1 ] =
0.215241895984875 ; xi [ 2 ] = 0.286174591792068 ; xi [ 3 ] =
0.335737519214422 ; xi [ 4 ] = 0.375121332878378 ; xi [ 5 ] =
0.408389134611989 ; xi [ 6 ] = 0.43751840220787 ; xi [ 7 ] = 0.46363433679088
; xi [ 8 ] = 0.487443966139235 ; xi [ 9 ] = 0.50942332960209 ; xi [ 10 ] =
0.529909720661557 ; xi [ 11 ] = 0.549151702327164 ; xi [ 12 ] =
0.567338257053817 ; xi [ 13 ] = 0.584616766106378 ; xi [ 14 ] =
0.601104617755991 ; xi [ 15 ] = 0.61689699000775 ; xi [ 16 ] =
0.63207223638606 ; xi [ 17 ] = 0.646695714894993 ; xi [ 18 ] =
0.660822574244419 ; xi [ 19 ] = 0.674499822837293 ; xi [ 20 ] =
0.687767892795788 ; xi [ 21 ] = 0.700661841106814 ; xi [ 22 ] =
0.713212285190975 ; xi [ 23 ] = 0.725446140909999 ; xi [ 24 ] =
0.737387211434295 ; xi [ 25 ] = 0.749056662017815 ; xi [ 26 ] =
0.760473406430107 ; xi [ 27 ] = 0.771654424224568 ; xi [ 28 ] =
0.782615023307232 ; xi [ 29 ] = 0.793369058840623 ; xi [ 30 ] =
0.80392911698997 ; xi [ 31 ] = 0.814306670135215 ; xi [ 32 ] =
0.824512208752291 ; xi [ 33 ] = 0.834555354086381 ; xi [ 34 ] =
0.844444954909153 ; xi [ 35 ] = 0.854189171008163 ; xi [ 36 ] =
0.863795545553308 ; xi [ 37 ] = 0.87327106808886 ; xi [ 38 ] =
0.882622229585165 ; xi [ 39 ] = 0.891855070732941 ; xi [ 40 ] =
0.900975224461221 ; xi [ 41 ] = 0.909987953496718 ; xi [ 42 ] =
0.91889818364959 ; xi [ 43 ] = 0.927710533401999 ; xi [ 44 ] =
0.936429340286575 ; xi [ 45 ] = 0.945058684468165 ; xi [ 46 ] =
0.953602409881086 ; xi [ 47 ] = 0.96206414322304 ; xi [ 48 ] =
0.970447311064224 ; xi [ 49 ] = 0.978755155294224 ; xi [ 50 ] =
0.986990747099062 ; xi [ 51 ] = 0.99515699963509 ; xi [ 52 ] =
1.00325667954467 ; xi [ 53 ] = 1.01129241744 ; xi [ 54 ] = 1.01926671746548 ;
xi [ 55 ] = 1.02718196603564 ; xi [ 56 ] = 1.03504043983344 ; xi [ 57 ] =
1.04284431314415 ; xi [ 58 ] = 1.05059566459093 ; xi [ 59 ] =
1.05829648333067 ; xi [ 60 ] = 1.06594867476212 ; xi [ 61 ] =
1.07355406579244 ; xi [ 62 ] = 1.0811144097034 ; xi [ 63 ] = 1.08863139065398
; xi [ 64 ] = 1.09610662785202 ; xi [ 65 ] = 1.10354167942464 ; xi [ 66 ] =
1.11093804601357 ; xi [ 67 ] = 1.11829717411934 ; xi [ 68 ] =
1.12562045921553 ; xi [ 69 ] = 1.13290924865253 ; xi [ 70 ] =
1.14016484436815 ; xi [ 71 ] = 1.14738850542085 ; xi [ 72 ] =
1.15458145035993 ; xi [ 73 ] = 1.16174485944561 ; xi [ 74 ] =
1.16887987673083 ; xi [ 75 ] = 1.17598761201545 ; xi [ 76 ] =
1.18306914268269 ; xi [ 77 ] = 1.19012551542669 ; xi [ 78 ] =
1.19715774787944 ; xi [ 79 ] = 1.20416683014438 ; xi [ 80 ] = 1.2111537262437
; xi [ 81 ] = 1.21811937548548 ; xi [ 82 ] = 1.22506469375653 ; xi [ 83 ] =
1.23199057474614 ; xi [ 84 ] = 1.23889789110569 ; xi [ 85 ] =
1.24578749554863 ; xi [ 86 ] = 1.2526602218949 ; xi [ 87 ] = 1.25951688606371
; xi [ 88 ] = 1.26635828701823 ; xi [ 89 ] = 1.27318520766536 ; xi [ 90 ] =
1.27999841571382 ; xi [ 91 ] = 1.28679866449324 ; xi [ 92 ] =
1.29358669373695 ; xi [ 93 ] = 1.30036323033084 ; xi [ 94 ] =
1.30712898903073 ; xi [ 95 ] = 1.31388467315022 ; xi [ 96 ] =
1.32063097522106 ; xi [ 97 ] = 1.32736857762793 ; xi [ 98 ] =
1.33409815321936 ; xi [ 99 ] = 1.3408203658964 ; xi [ 100 ] =
1.34753587118059 ; xi [ 101 ] = 1.35424531676263 ; xi [ 102 ] =
1.36094934303328 ; xi [ 103 ] = 1.36764858359748 ; xi [ 104 ] =
1.37434366577317 ; xi [ 105 ] = 1.38103521107586 ; xi [ 106 ] =
1.38772383568998 ; xi [ 107 ] = 1.39441015092814 ; xi [ 108 ] =
1.40109476367925 ; xi [ 109 ] = 1.4077782768464 ; xi [ 110 ] =
1.41446128977547 ; xi [ 111 ] = 1.42114439867531 ; xi [ 112 ] =
1.42782819703026 ; xi [ 113 ] = 1.43451327600589 ; xi [ 114 ] =
1.44120022484872 ; xi [ 115 ] = 1.44788963128058 ; xi [ 116 ] =
1.45458208188841 ; xi [ 117 ] = 1.46127816251028 ; xi [ 118 ] =
1.46797845861808 ; xi [ 119 ] = 1.47468355569786 ; xi [ 120 ] =
1.48139403962819 ; xi [ 121 ] = 1.48811049705745 ; xi [ 122 ] =
1.49483351578049 ; xi [ 123 ] = 1.50156368511546 ; xi [ 124 ] =
1.50830159628131 ; xi [ 125 ] = 1.51504784277671 ; xi [ 126 ] =
1.521803020761 ; xi [ 127 ] = 1.52856772943771 ; xi [ 128 ] =
1.53534257144151 ; xi [ 129 ] = 1.542128153229 ; xi [ 130 ] =
1.54892508547417 ; xi [ 131 ] = 1.55573398346918 ; xi [ 132 ] =
1.56255546753104 ; xi [ 133 ] = 1.56939016341512 ; xi [ 134 ] =
1.57623870273591 ; xi [ 135 ] = 1.58310172339603 ; xi [ 136 ] =
1.58997987002419 ; xi [ 137 ] = 1.59687379442279 ; xi [ 138 ] =
1.60378415602609 ; xi [ 139 ] = 1.61071162236983 ; xi [ 140 ] =
1.61765686957301 ; xi [ 141 ] = 1.62462058283303 ; xi [ 142 ] =
1.63160345693487 ; xi [ 143 ] = 1.63860619677555 ; xi [ 144 ] =
1.64562951790478 ; xi [ 145 ] = 1.65267414708306 ; xi [ 146 ] =
1.65974082285818 ; xi [ 147 ] = 1.66683029616166 ; xi [ 148 ] =
1.67394333092612 ; xi [ 149 ] = 1.68108070472517 ; xi [ 150 ] =
1.68824320943719 ; xi [ 151 ] = 1.69543165193456 ; xi [ 152 ] =
1.70264685479992 ; xi [ 153 ] = 1.7098896570713 ; xi [ 154 ] =
1.71716091501782 ; xi [ 155 ] = 1.72446150294804 ; xi [ 156 ] =
1.73179231405296 ; xi [ 157 ] = 1.73915426128591 ; xi [ 158 ] =
1.74654827828172 ; xi [ 159 ] = 1.75397532031767 ; xi [ 160 ] =
1.76143636531891 ; xi [ 161 ] = 1.76893241491127 ; xi [ 162 ] =
1.77646449552452 ; xi [ 163 ] = 1.78403365954944 ; xi [ 164 ] =
1.79164098655216 ; xi [ 165 ] = 1.79928758454972 ; xi [ 166 ] =
1.80697459135082 ; xi [ 167 ] = 1.81470317596628 ; xi [ 168 ] =
1.82247454009388 ; xi [ 169 ] = 1.83028991968276 ; xi [ 170 ] =
1.83815058658281 ; xi [ 171 ] = 1.84605785028518 ; xi [ 172 ] =
1.8540130597602 ; xi [ 173 ] = 1.86201760539967 ; xi [ 174 ] =
1.87007292107127 ; xi [ 175 ] = 1.878180486293 ; xi [ 176 ] =
1.88634182853678 ; xi [ 177 ] = 1.8945585256707 ; xi [ 178 ] =
1.90283220855043 ; xi [ 179 ] = 1.91116456377125 ; xi [ 180 ] =
1.91955733659319 ; xi [ 181 ] = 1.92801233405266 ; xi [ 182 ] =
1.93653142827569 ; xi [ 183 ] = 1.94511656000868 ; xi [ 184 ] =
1.95376974238465 ; xi [ 185 ] = 1.96249306494436 ; xi [ 186 ] =
1.97128869793366 ; xi [ 187 ] = 1.98015889690048 ; xi [ 188 ] =
1.98910600761744 ; xi [ 189 ] = 1.99813247135842 ; xi [ 190 ] =
2.00724083056053 ; xi [ 191 ] = 2.0164337349062 ; xi [ 192 ] =
2.02571394786385 ; xi [ 193 ] = 2.03508435372962 ; xi [ 194 ] =
2.04454796521753 ; xi [ 195 ] = 2.05410793165065 ; xi [ 196 ] =
2.06376754781173 ; xi [ 197 ] = 2.07353026351874 ; xi [ 198 ] =
2.0833996939983 ; xi [ 199 ] = 2.09337963113879 ; xi [ 200 ] =
2.10347405571488 ; xi [ 201 ] = 2.11368715068665 ; xi [ 202 ] =
2.12402331568952 ; xi [ 203 ] = 2.13448718284602 ; xi [ 204 ] =
2.14508363404789 ; xi [ 205 ] = 2.15581781987674 ; xi [ 206 ] =
2.16669518035431 ; xi [ 207 ] = 2.17772146774029 ; xi [ 208 ] =
2.18890277162636 ; xi [ 209 ] = 2.20024554661128 ; xi [ 210 ] =
2.21175664288416 ; xi [ 211 ] = 2.22344334009251 ; xi [ 212 ] =
2.23531338492992 ; xi [ 213 ] = 2.24737503294739 ; xi [ 214 ] =
2.25963709517379 ; xi [ 215 ] = 2.27210899022838 ; xi [ 216 ] =
2.28480080272449 ; xi [ 217 ] = 2.29772334890286 ; xi [ 218 ] =
2.31088825060137 ; xi [ 219 ] = 2.32430801887113 ; xi [ 220 ] =
2.33799614879653 ; xi [ 221 ] = 2.35196722737914 ; xi [ 222 ] =
2.36623705671729 ; xi [ 223 ] = 2.38082279517208 ; xi [ 224 ] =
2.39574311978193 ; xi [ 225 ] = 2.41101841390112 ; xi [ 226 ] =
2.42667098493715 ; xi [ 227 ] = 2.44272531820036 ; xi [ 228 ] =
2.4592083743347 ; xi [ 229 ] = 2.47614993967052 ; xi [ 230 ] =
2.49358304127105 ; xi [ 231 ] = 2.51154444162669 ; xi [ 232 ] =
2.53007523215985 ; xi [ 233 ] = 2.54922155032478 ; xi [ 234 ] =
2.56903545268184 ; xi [ 235 ] = 2.58957598670829 ; xi [ 236 ] =
2.61091051848882 ; xi [ 237 ] = 2.63311639363158 ; xi [ 238 ] =
2.65628303757674 ; xi [ 239 ] = 2.68051464328574 ; xi [ 240 ] =
2.70593365612306 ; xi [ 241 ] = 2.73268535904401 ; xi [ 242 ] =
2.76094400527999 ; xi [ 243 ] = 2.79092117400193 ; xi [ 244 ] =
2.82287739682644 ; xi [ 245 ] = 2.85713873087322 ; xi [ 246 ] =
2.89412105361341 ; xi [ 247 ] = 2.93436686720889 ; xi [ 248 ] =
2.97860327988184 ; xi [ 249 ] = 3.02783779176959 ; xi [ 250 ] =
3.08352613200214 ; xi [ 251 ] = 3.147889289518 ; xi [ 252 ] = 3.2245750520478
; xi [ 253 ] = 3.32024473383983 ; xi [ 254 ] = 3.44927829856143 ; xi [ 255 ]
= 3.65415288536101 ; xi [ 256 ] = 3.91075795952492 ; fitab = & tmp [ 0 ] ; }
for ( b_k = 0 ; b_k <= d ; b_k ++ ) { do { exitg1 = 0 ; csqey0xxgza ( rtDW .
ehmyrdjr0v , u32 ) ; i = ( int32_T ) ( ( u32 [ 1 ] >> 24U ) + 1U ) ; b_r = (
( ( real_T ) ( u32 [ 0 ] >> 3U ) * 1.6777216E+7 + ( real_T ) ( ( int32_T )
u32 [ 1 ] & 16777215 ) ) * 2.2204460492503131E-16 - 1.0 ) * xi [ i ] ; if (
muDoubleScalarAbs ( b_r ) <= xi [ i - 1 ] ) { exitg1 = 1 ; } else if ( i <
256 ) { do { exitg2 = 0 ; csqey0xxgza ( rtDW . ehmyrdjr0v , u32 ) ; x = ( (
real_T ) ( u32 [ 0 ] >> 5U ) * 6.7108864E+7 + ( real_T ) ( u32 [ 1 ] >> 6U )
) * 1.1102230246251565E-16 ; if ( x == 0.0 ) { if ( ! enplvw3xkz ( rtDW .
ehmyrdjr0v ) ) { rtDW . ehmyrdjr0v [ 0 ] = 5489U ; rtDW . ehmyrdjr0v [ 624 ]
= 624U ; } } else { exitg2 = 1 ; } } while ( exitg2 == 0 ) ; if ( ( fitab [ i
- 1 ] - fitab [ i ] ) * x + fitab [ i ] < muDoubleScalarExp ( - 0.5 * b_r *
b_r ) ) { exitg1 = 1 ; } } else { do { do { exitg2 = 0 ; csqey0xxgza ( rtDW .
ehmyrdjr0v , u32 ) ; x = ( ( real_T ) ( u32 [ 0 ] >> 5U ) * 6.7108864E+7 + (
real_T ) ( u32 [ 1 ] >> 6U ) ) * 1.1102230246251565E-16 ; if ( x == 0.0 ) {
if ( ! enplvw3xkz ( rtDW . ehmyrdjr0v ) ) { rtDW . ehmyrdjr0v [ 0 ] = 5489U ;
rtDW . ehmyrdjr0v [ 624 ] = 624U ; } } else { exitg2 = 1 ; } } while ( exitg2
== 0 ) ; x = muDoubleScalarLog ( x ) * 0.273661237329758 ; do { exitg2 = 0 ;
csqey0xxgza ( rtDW . ehmyrdjr0v , u32 ) ; d_u = ( ( real_T ) ( u32 [ 0 ] >>
5U ) * 6.7108864E+7 + ( real_T ) ( u32 [ 1 ] >> 6U ) ) *
1.1102230246251565E-16 ; if ( d_u == 0.0 ) { if ( ! enplvw3xkz ( rtDW .
ehmyrdjr0v ) ) { rtDW . ehmyrdjr0v [ 0 ] = 5489U ; rtDW . ehmyrdjr0v [ 624 ]
= 624U ; } } else { exitg2 = 1 ; } } while ( exitg2 == 0 ) ; } while ( ! ( -
2.0 * muDoubleScalarLog ( d_u ) > x * x ) ) ; if ( b_r < 0.0 ) { b_r = x -
3.65415288536101 ; } else { b_r = 3.65415288536101 - x ; } exitg1 = 1 ; } }
while ( exitg1 == 0 ) ; r -> data [ b_k ] = b_r ; } } static void bsiqjn4ttt
( klgljlxbmf * in1 , const klgljlxbmf * in2 ) { klgljlxbmf * in1_p ; int32_T
i ; int32_T loop_ub ; int32_T stride_0_0 ; int32_T stride_1_0 ; calykgtvmm (
& in1_p , 1 ) ; i = in1_p -> size [ 0 ] ; in1_p -> size [ 0 ] = in2 -> size [
0 ] == 1 ? in1 -> size [ 0 ] : in2 -> size [ 0 ] ; miyl1rq4uj ( in1_p , i ) ;
stride_0_0 = ( in1 -> size [ 0 ] != 1 ) ; stride_1_0 = ( in2 -> size [ 0 ] !=
1 ) ; loop_ub = in2 -> size [ 0 ] == 1 ? in1 -> size [ 0 ] : in2 -> size [ 0
] ; for ( i = 0 ; i < loop_ub ; i ++ ) { in1_p -> data [ i ] = in1 -> data [
i * stride_0_0 ] - in2 -> data [ i * stride_1_0 ] ; } i = in1 -> size [ 0 ] ;
in1 -> size [ 0 ] = in1_p -> size [ 0 ] ; miyl1rq4uj ( in1 , i ) ; loop_ub =
in1_p -> size [ 0 ] ; if ( loop_ub - 1 >= 0 ) { memcpy ( & in1 -> data [ 0 ]
, & in1_p -> data [ 0 ] , ( uint32_T ) loop_ub * sizeof ( real_T ) ) ; }
bdhoypqe5x ( & in1_p ) ; } static void cu5fmhs0ay ( klgljlxbmf * in1 , const
klgljlxbmf * in2 ) { klgljlxbmf * in2_p ; int32_T i ; int32_T loop_ub ;
int32_T stride_0_0 ; int32_T stride_1_0 ; calykgtvmm ( & in2_p , 1 ) ; i =
in2_p -> size [ 0 ] ; in2_p -> size [ 0 ] = in1 -> size [ 0 ] == 1 ? in2 ->
size [ 0 ] : in1 -> size [ 0 ] ; miyl1rq4uj ( in2_p , i ) ; stride_0_0 = (
in2 -> size [ 0 ] != 1 ) ; stride_1_0 = ( in1 -> size [ 0 ] != 1 ) ; loop_ub
= in1 -> size [ 0 ] == 1 ? in2 -> size [ 0 ] : in1 -> size [ 0 ] ; for ( i =
0 ; i < loop_ub ; i ++ ) { in2_p -> data [ i ] = in2 -> data [ i * stride_0_0
] + in1 -> data [ i * stride_1_0 ] ; } i = in1 -> size [ 0 ] ; in1 -> size [
0 ] = in2_p -> size [ 0 ] ; miyl1rq4uj ( in1 , i ) ; loop_ub = in2_p -> size
[ 0 ] ; if ( loop_ub - 1 >= 0 ) { memcpy ( & in1 -> data [ 0 ] , & in2_p ->
data [ 0 ] , ( uint32_T ) loop_ub * sizeof ( real_T ) ) ; } bdhoypqe5x ( &
in2_p ) ; } static void jlodq4k5z5m ( real_T varargin_1 , klgljlxbmf * r ) {
real_T b_r ; int32_T b_k ; int32_T d ; int32_T exitg1 ; uint32_T b_u [ 2 ] ;
b_k = r -> size [ 0 ] ; r -> size [ 0 ] = ( int32_T ) varargin_1 ; miyl1rq4uj
( r , b_k ) ; d = ( int32_T ) varargin_1 - 1 ; for ( b_k = 0 ; b_k <= d ; b_k
++ ) { do { exitg1 = 0 ; csqey0xxgza ( rtDW . ehmyrdjr0v , b_u ) ; b_r = ( (
real_T ) ( b_u [ 0 ] >> 5U ) * 6.7108864E+7 + ( real_T ) ( b_u [ 1 ] >> 6U )
) * 1.1102230246251565E-16 ; if ( b_r == 0.0 ) { if ( ! enplvw3xkz ( rtDW .
ehmyrdjr0v ) ) { rtDW . ehmyrdjr0v [ 0 ] = 5489U ; rtDW . ehmyrdjr0v [ 624 ]
= 624U ; } } else { exitg1 = 1 ; } } while ( exitg1 == 0 ) ; r -> data [ b_k
] = b_r ; } } static void offa2br1kh ( klgljlxbmf * in1 , const klgljlxbmf *
in2 , const klgljlxbmf * in3 ) { klgljlxbmf * in2_p ; int32_T i ; int32_T
loop_ub ; int32_T stride_0_0 ; int32_T stride_1_0 ; int32_T stride_2_0 ;
int32_T stride_3_0 ; calykgtvmm ( & in2_p , 1 ) ; i = in2_p -> size [ 0 ] ;
in2_p -> size [ 0 ] = ( ( in2 -> size [ 0 ] == 1 ? in1 -> size [ 0 ] : in2 ->
size [ 0 ] ) == 1 ? in3 -> size [ 0 ] : in2 -> size [ 0 ] == 1 ? in1 -> size
[ 0 ] : in2 -> size [ 0 ] ) == 1 ? in2 -> size [ 0 ] : ( in2 -> size [ 0 ] ==
1 ? in1 -> size [ 0 ] : in2 -> size [ 0 ] ) == 1 ? in3 -> size [ 0 ] : in2 ->
size [ 0 ] == 1 ? in1 -> size [ 0 ] : in2 -> size [ 0 ] ; miyl1rq4uj ( in2_p
, i ) ; stride_0_0 = ( in2 -> size [ 0 ] != 1 ) ; stride_1_0 = ( in3 -> size
[ 0 ] != 1 ) ; stride_2_0 = ( in1 -> size [ 0 ] != 1 ) ; stride_3_0 = ( in2
-> size [ 0 ] != 1 ) ; loop_ub = ( ( in2 -> size [ 0 ] == 1 ? in1 -> size [ 0
] : in2 -> size [ 0 ] ) == 1 ? in3 -> size [ 0 ] : in2 -> size [ 0 ] == 1 ?
in1 -> size [ 0 ] : in2 -> size [ 0 ] ) == 1 ? in2 -> size [ 0 ] : ( in2 ->
size [ 0 ] == 1 ? in1 -> size [ 0 ] : in2 -> size [ 0 ] ) == 1 ? in3 -> size
[ 0 ] : in2 -> size [ 0 ] == 1 ? in1 -> size [ 0 ] : in2 -> size [ 0 ] ; for
( i = 0 ; i < loop_ub ; i ++ ) { in2_p -> data [ i ] = ( in1 -> data [ i *
stride_2_0 ] - in2 -> data [ i * stride_3_0 ] ) * in3 -> data [ i *
stride_1_0 ] + in2 -> data [ i * stride_0_0 ] ; } i = in1 -> size [ 0 ] ; in1
-> size [ 0 ] = in2_p -> size [ 0 ] ; miyl1rq4uj ( in1 , i ) ; loop_ub =
in2_p -> size [ 0 ] ; if ( loop_ub - 1 >= 0 ) { memcpy ( & in1 -> data [ 0 ]
, & in2_p -> data [ 0 ] , ( uint32_T ) loop_ub * sizeof ( real_T ) ) ; }
bdhoypqe5x ( & in2_p ) ; } static void d2iciofqsp ( ecfe5we0wc * obj , const
real_T seed [ 6 ] , real_T xSol [ 6 ] , real_T * solutionInfo_Iterations ,
real_T * solutionInfo_RRAttempts , real_T * solutionInfo_Error , real_T *
solutionInfo_ExitFlag , char_T solutionInfo_Status_data [ ] , int32_T
solutionInfo_Status_size [ 2 ] ) { __m128d tmp_e ; __m128d tmp_i ; __m128d
tmp_m ; klgljlxbmf * lb ; klgljlxbmf * newseed ; klgljlxbmf * rn ; klgljlxbmf
* ub ; lnin0tndd2 * args ; m0caqfktx0 * obj_p ; m5kgub1d5k * b ; m5kgub1d5k *
tmp ; m5kgub1d5k * tmp_p ; ot4uovzkt0w * obj_e ; real_T c_xSol [ 6 ] ; real_T
ub_p [ 2 ] ; real_T err ; real_T iter ; real_T lb_p ; real_T tol ; gkb0igonbu
exitFlag ; gkb0igonbu exitFlagPrev ; int32_T c ; int32_T i ; int32_T loop_ub
; int32_T nx ; int32_T scalarLB ; int32_T vectorUB ; boolean_T y ; static
const char_T tmp_g [ 14 ] = { 'b' , 'e' , 's' , 't' , ' ' , 'a' , 'v' , 'a' ,
'i' , 'l' , 'a' , 'b' , 'l' , 'e' } ; static const char_T tmp_j [ 7 ] = { 's'
, 'u' , 'c' , 'c' , 'e' , 's' , 's' } ; boolean_T exitg1 ; boolean_T exitg2 ;
boolean_T guard1 = false ; boolean_T guard2 = false ; boolean_T guard3 =
false ; obj -> MaxNumIterationInternal = obj -> MaxNumIteration ; obj ->
MaxTimeInternal = obj -> MaxTime ; for ( i = 0 ; i < 6 ; i ++ ) { obj ->
SeedInternal [ i ] = seed [ i ] ; } tol = obj -> SolutionTolerance ;
ot2wucsrdj ( & obj -> TimeObj . StartTime . tv_sec , & obj -> TimeObj .
StartTime . tv_nsec ) ; ngq4y5c0bx ( obj , xSol , & exitFlag , & err , & iter
) ; * solutionInfo_RRAttempts = 0.0 ; * solutionInfo_Iterations = iter ; *
solutionInfo_Error = err ; exitFlagPrev = exitFlag ; calykgtvmm ( & newseed ,
1 ) ; calykgtvmm ( & ub , 1 ) ; calykgtvmm ( & lb , 1 ) ; calykgtvmm ( & rn ,
1 ) ; l45e0kru51 ( & b , 1 ) ; l45e0kru51 ( & tmp , 1 ) ; l45e0kru51 ( &
tmp_p , 1 ) ; exitg1 = false ; while ( ( ! exitg1 ) && ( obj -> RandomRestart
&& ( err > tol ) ) ) { obj -> MaxNumIterationInternal -= iter ; err =
ethycgs32k ( obj -> TimeObj . StartTime . tv_sec , obj -> TimeObj . StartTime
. tv_nsec ) ; obj -> MaxTimeInternal = obj -> MaxTime - err ; if ( obj ->
MaxNumIterationInternal <= 0.0 ) { exitFlag =
robotics_core_internal_NLPSolverExitFlags_IterationLimitExceeded ; } if ( (
exitFlag == robotics_core_internal_NLPSolverExitFlags_IterationLimitExceeded
) || ( exitFlag ==
robotics_core_internal_NLPSolverExitFlags_TimeLimitExceeded ) ) {
exitFlagPrev = exitFlag ; exitg1 = true ; } else { args = obj -> ExtraArgs ;
obj_p = args -> Robot ; loop_ub = newseed -> size [ 0 ] ; newseed -> size [ 0
] = ( int32_T ) obj_p -> PositionNumber ; miyl1rq4uj ( newseed , loop_ub ) ;
loop_ub = ( int32_T ) obj_p -> PositionNumber ; if ( loop_ub - 1 >= 0 ) {
memset ( & newseed -> data [ 0 ] , 0 , ( uint32_T ) loop_ub * sizeof ( real_T
) ) ; } err = obj_p -> NumBodies ; c = ( int32_T ) err - 1 ; for ( i = 0 ; i
<= c ; i ++ ) { err = obj_p -> PositionDoFMap [ i ] ; iter = obj_p ->
PositionDoFMap [ i + 9 ] ; if ( err <= iter ) { obj_e = obj_p -> Bodies [ i ]
-> JointInternal ; if ( ( int32_T ) obj_e -> PositionNumber == 0 ) { loop_ub
= ub -> size [ 0 ] ; ub -> size [ 0 ] = 1 ; miyl1rq4uj ( ub , loop_ub ) ; ub
-> data [ 0 ] = ( rtNaN ) ; } else { loop_ub = ub -> size [ 0 ] ; ub -> size
[ 0 ] = obj_e -> PositionLimitsInternal -> size [ 0 ] ; miyl1rq4uj ( ub ,
loop_ub ) ; loop_ub = obj_e -> PositionLimitsInternal -> size [ 0 ] ; for (
nx = 0 ; nx < loop_ub ; nx ++ ) { ub -> data [ nx ] = obj_e ->
PositionLimitsInternal -> data [ nx + obj_e -> PositionLimitsInternal -> size
[ 0 ] ] ; } loop_ub = lb -> size [ 0 ] ; lb -> size [ 0 ] = obj_e ->
PositionLimitsInternal -> size [ 0 ] ; miyl1rq4uj ( lb , loop_ub ) ; loop_ub
= obj_e -> PositionLimitsInternal -> size [ 0 ] ; for ( nx = 0 ; nx < loop_ub
; nx ++ ) { lb -> data [ nx ] = obj_e -> PositionLimitsInternal -> data [ nx
] ; } loop_ub = b -> size [ 0 ] ; b -> size [ 0 ] = lb -> size [ 0 ] ;
dw1xs30knw ( b , loop_ub ) ; loop_ub = lb -> size [ 0 ] ; for ( nx = 0 ; nx <
loop_ub ; nx ++ ) { b -> data [ nx ] = muDoubleScalarIsInf ( lb -> data [ nx
] ) ; } loop_ub = tmp -> size [ 0 ] ; tmp -> size [ 0 ] = lb -> size [ 0 ] ;
dw1xs30knw ( tmp , loop_ub ) ; loop_ub = lb -> size [ 0 ] ; for ( nx = 0 ; nx
< loop_ub ; nx ++ ) { tmp -> data [ nx ] = muDoubleScalarIsNaN ( lb -> data [
nx ] ) ; } loop_ub = b -> size [ 0 ] ; for ( nx = 0 ; nx < loop_ub ; nx ++ )
{ b -> data [ nx ] = ( ( ! b -> data [ nx ] ) && ( ! tmp -> data [ nx ] ) ) ;
} y = true ; loop_ub = 0 ; exitg2 = false ; while ( ( ! exitg2 ) && ( loop_ub
+ 1 <= b -> size [ 0 ] ) ) { if ( ! b -> data [ loop_ub ] ) { y = false ;
exitg2 = true ; } else { loop_ub ++ ; } } guard1 = false ; guard2 = false ;
guard3 = false ; if ( y ) { loop_ub = b -> size [ 0 ] ; b -> size [ 0 ] = ub
-> size [ 0 ] ; dw1xs30knw ( b , loop_ub ) ; loop_ub = ub -> size [ 0 ] ; for
( nx = 0 ; nx < loop_ub ; nx ++ ) { b -> data [ nx ] = muDoubleScalarIsInf (
ub -> data [ nx ] ) ; } loop_ub = tmp -> size [ 0 ] ; tmp -> size [ 0 ] = ub
-> size [ 0 ] ; dw1xs30knw ( tmp , loop_ub ) ; loop_ub = ub -> size [ 0 ] ;
for ( nx = 0 ; nx < loop_ub ; nx ++ ) { tmp -> data [ nx ] =
muDoubleScalarIsNaN ( ub -> data [ nx ] ) ; } loop_ub = b -> size [ 0 ] ; for
( nx = 0 ; nx < loop_ub ; nx ++ ) { b -> data [ nx ] = ( ( ! b -> data [ nx ]
) && ( ! tmp -> data [ nx ] ) ) ; } loop_ub = 0 ; exitg2 = false ; while ( (
! exitg2 ) && ( loop_ub + 1 <= b -> size [ 0 ] ) ) { if ( ! b -> data [
loop_ub ] ) { y = false ; exitg2 = true ; } else { loop_ub ++ ; } } if ( y )
{ jlodq4k5z5m ( obj_e -> PositionNumber , rn ) ; if ( ( ub -> size [ 0 ] ==
lb -> size [ 0 ] ) && ( ( ub -> size [ 0 ] == 1 ? lb -> size [ 0 ] : ub ->
size [ 0 ] ) == rn -> size [ 0 ] ) && ( ( rn -> size [ 0 ] == 1 ? ub -> size
[ 0 ] == 1 ? lb -> size [ 0 ] : ub -> size [ 0 ] : rn -> size [ 0 ] ) == lb
-> size [ 0 ] ) ) { loop_ub = ub -> size [ 0 ] ; ub -> size [ 0 ] = lb ->
size [ 0 ] ; miyl1rq4uj ( ub , loop_ub ) ; loop_ub = lb -> size [ 0 ] ;
scalarLB = ( lb -> size [ 0 ] / 2 ) << 1 ; vectorUB = scalarLB - 2 ; for ( nx
= 0 ; nx <= vectorUB ; nx += 2 ) { tmp_i = _mm_loadu_pd ( & ub -> data [ nx ]
) ; tmp_m = _mm_loadu_pd ( & lb -> data [ nx ] ) ; tmp_e = _mm_loadu_pd ( &
rn -> data [ nx ] ) ; _mm_storeu_pd ( & ub -> data [ nx ] , _mm_add_pd (
_mm_mul_pd ( _mm_sub_pd ( tmp_i , tmp_m ) , tmp_e ) , tmp_m ) ) ; } for ( nx
= scalarLB ; nx < loop_ub ; nx ++ ) { lb_p = lb -> data [ nx ] ; ub -> data [
nx ] = ( ub -> data [ nx ] - lb_p ) * rn -> data [ nx ] + lb_p ; } } else {
offa2br1kh ( ub , lb , rn ) ; } } else { guard3 = true ; } } else { guard3 =
true ; } if ( guard3 ) { loop_ub = b -> size [ 0 ] ; b -> size [ 0 ] = lb ->
size [ 0 ] ; dw1xs30knw ( b , loop_ub ) ; loop_ub = lb -> size [ 0 ] ; for (
nx = 0 ; nx < loop_ub ; nx ++ ) { b -> data [ nx ] = muDoubleScalarIsInf ( lb
-> data [ nx ] ) ; } loop_ub = tmp -> size [ 0 ] ; tmp -> size [ 0 ] = lb ->
size [ 0 ] ; dw1xs30knw ( tmp , loop_ub ) ; loop_ub = lb -> size [ 0 ] ; for
( nx = 0 ; nx < loop_ub ; nx ++ ) { tmp -> data [ nx ] = muDoubleScalarIsNaN
( lb -> data [ nx ] ) ; } loop_ub = b -> size [ 0 ] ; for ( nx = 0 ; nx <
loop_ub ; nx ++ ) { b -> data [ nx ] = ( ( ! b -> data [ nx ] ) && ( ! tmp ->
data [ nx ] ) ) ; } y = true ; loop_ub = 0 ; exitg2 = false ; while ( ( !
exitg2 ) && ( loop_ub + 1 <= b -> size [ 0 ] ) ) { if ( ! b -> data [ loop_ub
] ) { y = false ; exitg2 = true ; } else { loop_ub ++ ; } } if ( y ) {
loop_ub = tmp -> size [ 0 ] ; tmp -> size [ 0 ] = ub -> size [ 0 ] ;
dw1xs30knw ( tmp , loop_ub ) ; loop_ub = ub -> size [ 0 ] ; for ( nx = 0 ; nx
< loop_ub ; nx ++ ) { tmp -> data [ nx ] = muDoubleScalarIsInf ( ub -> data [
nx ] ) ; } loop_ub = b -> size [ 0 ] ; b -> size [ 0 ] = ub -> size [ 0 ] ;
dw1xs30knw ( b , loop_ub ) ; loop_ub = ub -> size [ 0 ] ; for ( nx = 0 ; nx <
loop_ub ; nx ++ ) { b -> data [ nx ] = muDoubleScalarIsNaN ( ub -> data [ nx
] ) ; } loop_ub = tmp_p -> size [ 0 ] ; tmp_p -> size [ 0 ] = tmp -> size [ 0
] ; dw1xs30knw ( tmp_p , loop_ub ) ; loop_ub = tmp -> size [ 0 ] ; for ( nx =
0 ; nx < loop_ub ; nx ++ ) { tmp_p -> data [ nx ] = ( tmp -> data [ nx ] || b
-> data [ nx ] ) ; } if ( pahyhnfmn2 ( tmp_p ) ) { ub_p [ 0 ] = lb -> size [
0 ] ; ub_p [ 1 ] = 1.0 ; inyjui0sxx ( ub_p , rn ) ; nx = rn -> size [ 0 ] - 1
; loop_ub = ub -> size [ 0 ] ; ub -> size [ 0 ] = rn -> size [ 0 ] ;
miyl1rq4uj ( ub , loop_ub ) ; for ( loop_ub = 0 ; loop_ub <= nx ; loop_ub ++
) { ub -> data [ loop_ub ] = muDoubleScalarAbs ( rn -> data [ loop_ub ] ) ; }
if ( lb -> size [ 0 ] == ub -> size [ 0 ] ) { loop_ub = ub -> size [ 0 ] ; ub
-> size [ 0 ] = lb -> size [ 0 ] ; miyl1rq4uj ( ub , loop_ub ) ; loop_ub = lb
-> size [ 0 ] ; scalarLB = ( lb -> size [ 0 ] / 2 ) << 1 ; vectorUB =
scalarLB - 2 ; for ( nx = 0 ; nx <= vectorUB ; nx += 2 ) { tmp_i =
_mm_loadu_pd ( & lb -> data [ nx ] ) ; tmp_m = _mm_loadu_pd ( & ub -> data [
nx ] ) ; _mm_storeu_pd ( & ub -> data [ nx ] , _mm_add_pd ( tmp_i , tmp_m ) )
; } for ( nx = scalarLB ; nx < loop_ub ; nx ++ ) { ub -> data [ nx ] += lb ->
data [ nx ] ; } } else { cu5fmhs0ay ( ub , lb ) ; } } else { guard2 = true ;
} } else { guard2 = true ; } } if ( guard2 ) { loop_ub = tmp -> size [ 0 ] ;
tmp -> size [ 0 ] = lb -> size [ 0 ] ; dw1xs30knw ( tmp , loop_ub ) ; loop_ub
= lb -> size [ 0 ] ; for ( nx = 0 ; nx < loop_ub ; nx ++ ) { tmp -> data [ nx
] = muDoubleScalarIsInf ( lb -> data [ nx ] ) ; } loop_ub = b -> size [ 0 ] ;
b -> size [ 0 ] = lb -> size [ 0 ] ; dw1xs30knw ( b , loop_ub ) ; loop_ub =
lb -> size [ 0 ] ; for ( nx = 0 ; nx < loop_ub ; nx ++ ) { b -> data [ nx ] =
muDoubleScalarIsNaN ( lb -> data [ nx ] ) ; } loop_ub = tmp_p -> size [ 0 ] ;
tmp_p -> size [ 0 ] = tmp -> size [ 0 ] ; dw1xs30knw ( tmp_p , loop_ub ) ;
loop_ub = tmp -> size [ 0 ] ; for ( nx = 0 ; nx < loop_ub ; nx ++ ) { tmp_p
-> data [ nx ] = ( tmp -> data [ nx ] || b -> data [ nx ] ) ; } if (
pahyhnfmn2 ( tmp_p ) ) { loop_ub = b -> size [ 0 ] ; b -> size [ 0 ] = ub ->
size [ 0 ] ; dw1xs30knw ( b , loop_ub ) ; loop_ub = ub -> size [ 0 ] ; for (
nx = 0 ; nx < loop_ub ; nx ++ ) { b -> data [ nx ] = muDoubleScalarIsInf ( ub
-> data [ nx ] ) ; } loop_ub = tmp -> size [ 0 ] ; tmp -> size [ 0 ] = ub ->
size [ 0 ] ; dw1xs30knw ( tmp , loop_ub ) ; loop_ub = ub -> size [ 0 ] ; for
( nx = 0 ; nx < loop_ub ; nx ++ ) { tmp -> data [ nx ] = muDoubleScalarIsNaN
( ub -> data [ nx ] ) ; } loop_ub = b -> size [ 0 ] ; for ( nx = 0 ; nx <
loop_ub ; nx ++ ) { b -> data [ nx ] = ( ( ! b -> data [ nx ] ) && ( ! tmp ->
data [ nx ] ) ) ; } y = true ; loop_ub = 0 ; exitg2 = false ; while ( ( !
exitg2 ) && ( loop_ub + 1 <= b -> size [ 0 ] ) ) { if ( ! b -> data [ loop_ub
] ) { y = false ; exitg2 = true ; } else { loop_ub ++ ; } } if ( y ) { ub_p [
0 ] = ub -> size [ 0 ] ; ub_p [ 1 ] = 1.0 ; inyjui0sxx ( ub_p , rn ) ; nx =
rn -> size [ 0 ] - 1 ; loop_ub = lb -> size [ 0 ] ; lb -> size [ 0 ] = rn ->
size [ 0 ] ; miyl1rq4uj ( lb , loop_ub ) ; for ( loop_ub = 0 ; loop_ub <= nx
; loop_ub ++ ) { lb -> data [ loop_ub ] = muDoubleScalarAbs ( rn -> data [
loop_ub ] ) ; } if ( ub -> size [ 0 ] == lb -> size [ 0 ] ) { loop_ub = ub ->
size [ 0 ] ; scalarLB = ( ub -> size [ 0 ] / 2 ) << 1 ; vectorUB = scalarLB -
2 ; for ( nx = 0 ; nx <= vectorUB ; nx += 2 ) { tmp_i = _mm_loadu_pd ( & ub
-> data [ nx ] ) ; tmp_m = _mm_loadu_pd ( & lb -> data [ nx ] ) ;
_mm_storeu_pd ( & ub -> data [ nx ] , _mm_sub_pd ( tmp_i , tmp_m ) ) ; } for
( nx = scalarLB ; nx < loop_ub ; nx ++ ) { ub -> data [ nx ] -= lb -> data [
nx ] ; } } else { bsiqjn4ttt ( ub , lb ) ; } } else { guard1 = true ; } }
else { guard1 = true ; } } if ( guard1 ) { ub_p [ 0 ] = ub -> size [ 0 ] ;
ub_p [ 1 ] = 1.0 ; inyjui0sxx ( ub_p , ub ) ; } } if ( err > iter ) { loop_ub
= 0 ; nx = 0 ; } else { loop_ub = ( int32_T ) err - 1 ; nx = ( int32_T ) iter
; } scalarLB = nx - loop_ub ; for ( nx = 0 ; nx < scalarLB ; nx ++ ) {
newseed -> data [ loop_ub + nx ] = ub -> data [ nx ] ; } } } for ( nx = 0 ;
nx < 6 ; nx ++ ) { obj -> SeedInternal [ nx ] = newseed -> data [ nx ] ; }
ngq4y5c0bx ( obj , c_xSol , & exitFlag , & err , & iter ) ; if ( err < *
solutionInfo_Error ) { for ( i = 0 ; i < 6 ; i ++ ) { xSol [ i ] = c_xSol [ i
] ; } * solutionInfo_Error = err ; exitFlagPrev = exitFlag ; } ( *
solutionInfo_RRAttempts ) ++ ; * solutionInfo_Iterations += iter ; } }
naapodi4lu ( & tmp_p ) ; naapodi4lu ( & tmp ) ; naapodi4lu ( & b ) ;
bdhoypqe5x ( & rn ) ; bdhoypqe5x ( & lb ) ; bdhoypqe5x ( & ub ) ; bdhoypqe5x
( & newseed ) ; * solutionInfo_ExitFlag = exitFlagPrev ; if ( *
solutionInfo_Error < tol ) { solutionInfo_Status_size [ 0 ] = 1 ;
solutionInfo_Status_size [ 1 ] = 7 ; for ( nx = 0 ; nx < 7 ; nx ++ ) {
solutionInfo_Status_data [ nx ] = tmp_j [ nx ] ; } } else {
solutionInfo_Status_size [ 0 ] = 1 ; solutionInfo_Status_size [ 1 ] = 14 ;
for ( nx = 0 ; nx < 14 ; nx ++ ) { solutionInfo_Status_data [ nx ] = tmp_g [
nx ] ; } } } static void bev1y1zpzb ( pr5ntxwpwx * * pEmxArray , int32_T
numDimensions ) { pr5ntxwpwx * emxArray ; int32_T i ; * pEmxArray = (
pr5ntxwpwx * ) malloc ( sizeof ( pr5ntxwpwx ) ) ; emxArray = * pEmxArray ;
emxArray -> data = ( uint32_T * ) NULL ; emxArray -> numDimensions =
numDimensions ; emxArray -> size = ( int32_T * ) malloc ( sizeof ( int32_T )
* ( uint32_T ) numDimensions ) ; emxArray -> allocatedSize = 0 ; emxArray ->
canFreeData = true ; for ( i = 0 ; i < numDimensions ; i ++ ) { emxArray ->
size [ i ] = 0 ; } } static void amnzbgdo2d ( pr5ntxwpwx * emxArray , int32_T
oldNumel ) { int32_T i ; int32_T newNumel ; void * newData ; if ( oldNumel <
0 ) { oldNumel = 0 ; } newNumel = 1 ; for ( i = 0 ; i < emxArray ->
numDimensions ; i ++ ) { newNumel *= emxArray -> size [ i ] ; } if ( newNumel
> emxArray -> allocatedSize ) { i = emxArray -> allocatedSize ; if ( i < 16 )
{ i = 16 ; } while ( i < newNumel ) { if ( i > 1073741823 ) { i = MAX_int32_T
; } else { i <<= 1 ; } } newData = calloc ( ( uint32_T ) i , sizeof (
uint32_T ) ) ; if ( emxArray -> data != NULL ) { memcpy ( newData , emxArray
-> data , sizeof ( uint32_T ) * ( uint32_T ) oldNumel ) ; if ( emxArray ->
canFreeData ) { free ( emxArray -> data ) ; } } emxArray -> data = ( uint32_T
* ) newData ; emxArray -> allocatedSize = i ; emxArray -> canFreeData = true
; } } static void migotnoom2 ( pr5ntxwpwx * * pEmxArray ) { if ( * pEmxArray
!= ( pr5ntxwpwx * ) NULL ) { if ( ( ( * pEmxArray ) -> data != ( uint32_T * )
NULL ) && ( * pEmxArray ) -> canFreeData ) { free ( ( * pEmxArray ) -> data )
; } free ( ( * pEmxArray ) -> size ) ; free ( * pEmxArray ) ; * pEmxArray = (
pr5ntxwpwx * ) NULL ; } } static void c4f0e12yvkr ( carpjyrt23 * obj , real_T
initialGuess [ 6 ] , real_T * solutionInfo_Iterations , real_T *
solutionInfo_NumRandomRestarts , real_T * solutionInfo_PoseErrorNorm , real_T
* solutionInfo_ExitFlag , char_T solutionInfo_Status_data [ ] , int32_T
solutionInfo_Status_size [ 2 ] ) { alyuhn31fbfz * body ; e1n154mvbh *
endEffectorName ; klgljlxbmf * bodyIndices ; klgljlxbmf * e ; klgljlxbmf *
limits ; klgljlxbmf * positionIndices ; m0caqfktx0 * obj_p ; ncvluzgkod * h ;
pr5ntxwpwx * y ; real_T qvSolRaw [ 6 ] ; real_T a ; real_T apnd ; real_T bid
; real_T cdiff ; real_T d ; real_T ndbl ; real_T numPositions ; int32_T
indicesUpperBoundViolation_data [ 6 ] ; int32_T tmp_data [ 6 ] ; int32_T b_k
; int32_T c ; int32_T i ; int32_T indicesUpperBoundViolation ; int32_T
indicesUpperBoundViolation_size_idx_0 ; int32_T nm1d2 ; int32_T tmp_size ;
boolean_T lbOK [ 6 ] ; boolean_T ubOK [ 6 ] ; boolean_T ubOK_p [ 6 ] ;
boolean_T exitg1 ; boolean_T guard1 = false ; boolean_T y_p ; obj_p = obj ->
RigidBodyTreeInternal ; calykgtvmm ( & limits , 2 ) ; lij5c2bvo0 ( obj_p ,
limits ) ; if ( limits -> size [ 0 ] == 6 ) { for ( i = 0 ; i < 6 ; i ++ ) {
ubOK [ i ] = ( initialGuess [ i ] <= limits -> data [ i + limits -> size [ 0
] ] + 4.4408920985006262E-16 ) ; } } else { lh3fakbjip ( ubOK , initialGuess
, limits ) ; } if ( limits -> size [ 0 ] == 6 ) { for ( i = 0 ; i < 6 ; i ++
) { lbOK [ i ] = ( initialGuess [ i ] >= limits -> data [ i ] -
4.4408920985006262E-16 ) ; } } else { offa2br1khoegki ( lbOK , initialGuess ,
limits ) ; } y_p = true ; b_k = 0 ; exitg1 = false ; while ( ( ! exitg1 ) &&
( b_k < 6 ) ) { if ( ! ubOK [ b_k ] ) { y_p = false ; exitg1 = true ; } else
{ b_k ++ ; } } guard1 = false ; if ( y_p ) { b_k = 0 ; exitg1 = false ; while
( ( ! exitg1 ) && ( b_k < 6 ) ) { if ( ! lbOK [ b_k ] ) { y_p = false ;
exitg1 = true ; } else { b_k ++ ; } } if ( y_p ) { } else { guard1 = true ; }
} else { guard1 = true ; } if ( guard1 ) { for ( i = 0 ; i < 6 ; i ++ ) {
ubOK_p [ i ] = ! ubOK [ i ] ; } mfmaegtoxt ( ubOK_p , tmp_data , & tmp_size )
; indicesUpperBoundViolation_size_idx_0 = tmp_size ; b_k = tmp_size ; if (
b_k - 1 >= 0 ) { memcpy ( & indicesUpperBoundViolation_data [ 0 ] , &
tmp_data [ 0 ] , ( uint32_T ) b_k * sizeof ( int32_T ) ) ; } for ( i = 0 ; i
< indicesUpperBoundViolation_size_idx_0 ; i ++ ) { indicesUpperBoundViolation
= indicesUpperBoundViolation_data [ i ] ; initialGuess [
indicesUpperBoundViolation - 1 ] = limits -> data [ (
indicesUpperBoundViolation + limits -> size [ 0 ] ) - 1 ] ; } for ( i = 0 ; i
< 6 ; i ++ ) { ubOK [ i ] = ! lbOK [ i ] ; } mfmaegtoxt ( ubOK , tmp_data , &
tmp_size ) ; indicesUpperBoundViolation_size_idx_0 = tmp_size ; b_k =
tmp_size ; if ( b_k - 1 >= 0 ) { memcpy ( & indicesUpperBoundViolation_data [
0 ] , & tmp_data [ 0 ] , ( uint32_T ) b_k * sizeof ( int32_T ) ) ; } for ( i
= 0 ; i < indicesUpperBoundViolation_size_idx_0 ; i ++ ) {
indicesUpperBoundViolation = indicesUpperBoundViolation_data [ i ] ;
initialGuess [ indicesUpperBoundViolation - 1 ] = limits -> data [
indicesUpperBoundViolation - 1 ] ; } } d2iciofqsp ( obj -> Solver ,
initialGuess , qvSolRaw , solutionInfo_Iterations ,
solutionInfo_NumRandomRestarts , solutionInfo_PoseErrorNorm ,
solutionInfo_ExitFlag , solutionInfo_Status_data , solutionInfo_Status_size )
; obj_p = obj -> RigidBodyTreeInternal ; m2n1niwoq4 ( & endEffectorName , 2 )
; i = endEffectorName -> size [ 0 ] * endEffectorName -> size [ 1 ] ;
endEffectorName -> size [ 0 ] = 1 ; endEffectorName -> size [ 1 ] = obj ->
Solver -> ExtraArgs -> BodyName -> size [ 1 ] ; e3n3mnwk4b ( endEffectorName
, i ) ; b_k = obj -> Solver -> ExtraArgs -> BodyName -> size [ 1 ] ; for ( i
= 0 ; i < b_k ; i ++ ) { endEffectorName -> data [ i ] = obj -> Solver ->
ExtraArgs -> BodyName -> data [ i ] ; } calykgtvmm ( & bodyIndices , 1 ) ; i
= bodyIndices -> size [ 0 ] ; bodyIndices -> size [ 0 ] = ( int32_T ) obj_p
-> NumBodies ; miyl1rq4uj ( bodyIndices , i ) ; b_k = ( int32_T ) obj_p ->
NumBodies ; if ( b_k - 1 >= 0 ) { memset ( & bodyIndices -> data [ 0 ] , 0 ,
( uint32_T ) b_k * sizeof ( real_T ) ) ; } bid = jjyjgxfdua ( obj_p ,
endEffectorName ) ; fagcrr3avn ( & endEffectorName ) ; if ( bid == 0.0 ) { i
= bodyIndices -> size [ 0 ] ; bodyIndices -> size [ 0 ] = 1 ; miyl1rq4uj (
bodyIndices , i ) ; bodyIndices -> data [ 0 ] = 0.0 ; } else { body = obj_p
-> Bodies [ ( int32_T ) bid - 1 ] ; bid = 1.0 ; while ( body -> ParentIndex
!= 0.0 ) { bodyIndices -> data [ ( int32_T ) bid - 1 ] = body -> Index ; body
= obj_p -> Bodies [ ( int32_T ) body -> ParentIndex - 1 ] ; bid ++ ; } if (
bid - 1.0 < 1.0 ) { indicesUpperBoundViolation_size_idx_0 = 0 ; } else {
indicesUpperBoundViolation_size_idx_0 = ( int32_T ) ( bid - 1.0 ) ; } i =
bodyIndices -> size [ 0 ] ; bodyIndices -> size [ 0 ] =
indicesUpperBoundViolation_size_idx_0 + 2 ; miyl1rq4uj ( bodyIndices , i ) ;
bodyIndices -> data [ indicesUpperBoundViolation_size_idx_0 ] = body -> Index
; bodyIndices -> data [ indicesUpperBoundViolation_size_idx_0 + 1 ] = 0.0 ; }
obj_p = obj -> RigidBodyTreeInternal ; indicesUpperBoundViolation =
bodyIndices -> size [ 0 ] - 1 ; indicesUpperBoundViolation_size_idx_0 = 0 ;
for ( b_k = 0 ; b_k <= indicesUpperBoundViolation ; b_k ++ ) { if (
bodyIndices -> data [ b_k ] != 0.0 ) { indicesUpperBoundViolation_size_idx_0
++ ; } } ks3ealo3sd ( & h , 1 ) ; i = h -> size [ 0 ] ; h -> size [ 0 ] =
indicesUpperBoundViolation_size_idx_0 ; os5bv5kg0d ( h , i ) ;
indicesUpperBoundViolation_size_idx_0 = 0 ; for ( b_k = 0 ; b_k <=
indicesUpperBoundViolation ; b_k ++ ) { if ( bodyIndices -> data [ b_k ] !=
0.0 ) { h -> data [ indicesUpperBoundViolation_size_idx_0 ] = b_k + 1 ;
indicesUpperBoundViolation_size_idx_0 ++ ; } } i = limits -> size [ 0 ] *
limits -> size [ 1 ] ; limits -> size [ 0 ] = h -> size [ 0 ] ; limits ->
size [ 1 ] = 2 ; miyl1rq4uj ( limits , i ) ; b_k = h -> size [ 0 ] ; for ( i
= 0 ; i < 2 ; i ++ ) { for ( indicesUpperBoundViolation = 0 ;
indicesUpperBoundViolation < b_k ; indicesUpperBoundViolation ++ ) { limits
-> data [ indicesUpperBoundViolation + limits -> size [ 0 ] * i ] = obj_p ->
PositionDoFMap [ ( ( int32_T ) bodyIndices -> data [ h -> data [
indicesUpperBoundViolation ] - 1 ] + 9 * i ) - 1 ] ; } } preyilpnl4 ( & h ) ;
bdhoypqe5x ( & bodyIndices ) ; calykgtvmm ( & positionIndices , 2 ) ; i =
positionIndices -> size [ 0 ] * positionIndices -> size [ 1 ] ;
positionIndices -> size [ 0 ] = 1 ; positionIndices -> size [ 1 ] = ( int32_T
) obj_p -> PositionNumber ; miyl1rq4uj ( positionIndices , i ) ; b_k = (
int32_T ) obj_p -> PositionNumber ; if ( b_k - 1 >= 0 ) { memset ( &
positionIndices -> data [ 0 ] , 0 , ( uint32_T ) b_k * sizeof ( real_T ) ) ;
} bid = 0.0 ; indicesUpperBoundViolation_size_idx_0 = limits -> size [ 0 ] -
1 ; calykgtvmm ( & e , 2 ) ; bev1y1zpzb ( & y , 2 ) ; for (
indicesUpperBoundViolation = 0 ; indicesUpperBoundViolation <=
indicesUpperBoundViolation_size_idx_0 ; indicesUpperBoundViolation ++ ) {
numPositions = ( limits -> data [ indicesUpperBoundViolation + limits -> size
[ 0 ] ] - limits -> data [ indicesUpperBoundViolation ] ) + 1.0 ; if (
numPositions > 0.0 ) { if ( numPositions < 1.0 ) { y -> size [ 0 ] = 1 ; y ->
size [ 1 ] = 0 ; } else { i = y -> size [ 0 ] * y -> size [ 1 ] ; y -> size [
0 ] = 1 ; y -> size [ 1 ] = ( int32_T ) ( numPositions - 1.0 ) + 1 ;
amnzbgdo2d ( y , i ) ; b_k = ( int32_T ) ( numPositions - 1.0 ) ; for ( i = 0
; i <= b_k ; i ++ ) { y -> data [ i ] = ( uint32_T ) i + 1U ; } } a = limits
-> data [ indicesUpperBoundViolation ] ; d = limits -> data [
indicesUpperBoundViolation + limits -> size [ 0 ] ] ; if (
muDoubleScalarIsNaN ( a ) || muDoubleScalarIsNaN ( d ) ) { i = e -> size [ 0
] * e -> size [ 1 ] ; e -> size [ 0 ] = 1 ; e -> size [ 1 ] = 1 ; miyl1rq4uj
( e , i ) ; e -> data [ 0 ] = ( rtNaN ) ; } else if ( limits -> data [
indicesUpperBoundViolation + limits -> size [ 0 ] ] < limits -> data [
indicesUpperBoundViolation ] ) { e -> size [ 0 ] = 1 ; e -> size [ 1 ] = 0 ;
} else if ( ( muDoubleScalarIsInf ( a ) || muDoubleScalarIsInf ( d ) ) && (
limits -> data [ indicesUpperBoundViolation + limits -> size [ 0 ] ] ==
limits -> data [ indicesUpperBoundViolation ] ) ) { i = e -> size [ 0 ] * e
-> size [ 1 ] ; e -> size [ 0 ] = 1 ; e -> size [ 1 ] = 1 ; miyl1rq4uj ( e ,
i ) ; e -> data [ 0 ] = ( rtNaN ) ; } else if ( muDoubleScalarFloor ( a ) ==
limits -> data [ indicesUpperBoundViolation ] ) { a = limits -> data [
indicesUpperBoundViolation ] ; d = limits -> data [
indicesUpperBoundViolation + limits -> size [ 0 ] ] ; i = e -> size [ 0 ] * e
-> size [ 1 ] ; e -> size [ 0 ] = 1 ; e -> size [ 1 ] = ( int32_T ) ( d - a )
+ 1 ; miyl1rq4uj ( e , i ) ; b_k = ( int32_T ) ( d - a ) ; for ( i = 0 ; i <=
b_k ; i ++ ) { e -> data [ i ] = a + ( real_T ) i ; } } else { ndbl =
muDoubleScalarFloor ( ( limits -> data [ indicesUpperBoundViolation + limits
-> size [ 0 ] ] - limits -> data [ indicesUpperBoundViolation ] ) + 0.5 ) ;
apnd = limits -> data [ indicesUpperBoundViolation ] + ndbl ; cdiff = apnd -
limits -> data [ indicesUpperBoundViolation + limits -> size [ 0 ] ] ; if (
muDoubleScalarAbs ( cdiff ) < 4.4408920985006262E-16 * muDoubleScalarMax (
muDoubleScalarAbs ( a ) , muDoubleScalarAbs ( d ) ) ) { ndbl ++ ; apnd =
limits -> data [ indicesUpperBoundViolation + limits -> size [ 0 ] ] ; } else
if ( cdiff > 0.0 ) { apnd = ( ndbl - 1.0 ) + limits -> data [
indicesUpperBoundViolation ] ; } else { ndbl ++ ; } if ( ndbl >= 0.0 ) { i =
( int32_T ) ndbl ; } else { i = 0 ; } b_k = e -> size [ 0 ] * e -> size [ 1 ]
; e -> size [ 0 ] = 1 ; e -> size [ 1 ] = i ; miyl1rq4uj ( e , b_k ) ; if ( i
> 0 ) { e -> data [ 0 ] = limits -> data [ indicesUpperBoundViolation ] ; if
( i > 1 ) { e -> data [ i - 1 ] = apnd ; nm1d2 = ( int32_T ) ( ( uint32_T ) (
i - 1 ) >> 1 ) ; c = nm1d2 - 2 ; for ( b_k = 0 ; b_k <= c ; b_k ++ ) { e ->
data [ b_k + 1 ] = ( real_T ) ( b_k + 1 ) + a ; e -> data [ ( i - b_k ) - 2 ]
= apnd - ( real_T ) ( b_k + 1 ) ; } if ( nm1d2 << 1 == i - 1 ) { e -> data [
nm1d2 ] = ( limits -> data [ indicesUpperBoundViolation ] + apnd ) / 2.0 ; }
else { e -> data [ nm1d2 ] = limits -> data [ indicesUpperBoundViolation ] +
( real_T ) nm1d2 ; e -> data [ nm1d2 + 1 ] = apnd - ( real_T ) nm1d2 ; } } }
} b_k = e -> size [ 1 ] ; for ( i = 0 ; i < b_k ; i ++ ) { positionIndices ->
data [ ( int32_T ) ( bid + ( real_T ) y -> data [ i ] ) - 1 ] = e -> data [ i
] ; } bid += numPositions ; } } migotnoom2 ( & y ) ; bdhoypqe5x ( & e ) ;
bdhoypqe5x ( & limits ) ; if ( bid < 1.0 ) { positionIndices -> size [ 1 ] =
0 ; } else { i = positionIndices -> size [ 0 ] * positionIndices -> size [ 1
] ; positionIndices -> size [ 1 ] = ( int32_T ) bid ; miyl1rq4uj (
positionIndices , i ) ; } b_k = positionIndices -> size [ 1 ] ; for ( i = 0 ;
i < b_k ; i ++ ) { bid = positionIndices -> data [ i ] ; initialGuess [ (
int32_T ) bid - 1 ] = qvSolRaw [ ( int32_T ) bid - 1 ] ; } bdhoypqe5x ( &
positionIndices ) ; } static void gai2oub5e3 ( carpjyrt23 * obj , const
real_T tform [ 16 ] , const real_T weights [ 6 ] , const real_T initialGuess
[ 6 ] , real_T QSol [ 6 ] , real_T * solutionInfo_Iterations , real_T *
solutionInfo_PoseErrorNorm , real_T * solutionInfo_ExitFlag , char_T
solutionInfo_Status_data [ ] , int32_T solutionInfo_Status_size [ 2 ] ) {
lnin0tndd2 * args ; real_T weightMatrix [ 36 ] ; real_T expl_temp ; int32_T i
; static const char_T tmp [ 12 ] = { 'e' , 'n' , 'd' , '_' , 'e' , 'f' , 'f'
, 'e' , 'c' , 't' , 'o' , 'r' } ; memset ( & weightMatrix [ 0 ] , 0 , 36U *
sizeof ( real_T ) ) ; for ( i = 0 ; i < 6 ; i ++ ) { weightMatrix [ i + 6 * i
] = weights [ i ] ; } args = obj -> Solver -> ExtraArgs ; for ( i = 0 ; i <
36 ; i ++ ) { args -> WeightMatrix [ i ] = weightMatrix [ i ] ; } i = args ->
BodyName -> size [ 0 ] * args -> BodyName -> size [ 1 ] ; args -> BodyName ->
size [ 0 ] = 1 ; args -> BodyName -> size [ 1 ] = 12 ; e3n3mnwk4b ( args ->
BodyName , i ) ; for ( i = 0 ; i < 12 ; i ++ ) { args -> BodyName -> data [ i
] = tmp [ i ] ; } for ( i = 0 ; i < 16 ; i ++ ) { args -> Tform [ i ] = tform
[ i ] ; } for ( i = 0 ; i < 6 ; i ++ ) { QSol [ i ] = initialGuess [ i ] ; }
c4f0e12yvkr ( obj , QSol , solutionInfo_Iterations , & expl_temp ,
solutionInfo_PoseErrorNorm , solutionInfo_ExitFlag , solutionInfo_Status_data
, solutionInfo_Status_size ) ; } static void jarne05rzi ( hzpfvcreu1 * *
pEmxArray , int32_T numDimensions ) { hzpfvcreu1 * emxArray ; int32_T i ; *
pEmxArray = ( hzpfvcreu1 * ) malloc ( sizeof ( hzpfvcreu1 ) ) ; emxArray = *
pEmxArray ; emxArray -> data = ( bkyb12klys * ) NULL ; emxArray ->
numDimensions = numDimensions ; emxArray -> size = ( int32_T * ) malloc (
sizeof ( int32_T ) * ( uint32_T ) numDimensions ) ; emxArray -> allocatedSize
= 0 ; emxArray -> canFreeData = true ; for ( i = 0 ; i < numDimensions ; i ++
) { emxArray -> size [ i ] = 0 ; } } static void f33velrxf0 ( hzpfvcreu1 *
emxArray , int32_T oldNumel ) { int32_T i ; int32_T newNumel ; void * newData
; if ( oldNumel < 0 ) { oldNumel = 0 ; } newNumel = 1 ; for ( i = 0 ; i <
emxArray -> numDimensions ; i ++ ) { newNumel *= emxArray -> size [ i ] ; }
if ( newNumel > emxArray -> allocatedSize ) { i = emxArray -> allocatedSize ;
if ( i < 16 ) { i = 16 ; } while ( i < newNumel ) { if ( i > 1073741823 ) { i
= MAX_int32_T ; } else { i <<= 1 ; } } newData = calloc ( ( uint32_T ) i ,
sizeof ( bkyb12klys ) ) ; if ( emxArray -> data != NULL ) { memcpy ( newData
, emxArray -> data , sizeof ( bkyb12klys ) * ( uint32_T ) oldNumel ) ; if (
emxArray -> canFreeData ) { free ( emxArray -> data ) ; } } emxArray -> data
= ( bkyb12klys * ) newData ; emxArray -> allocatedSize = i ; emxArray ->
canFreeData = true ; } } static void cnixr4pd3a ( const ot4uovzkt0 * obj ,
real_T ax [ 3 ] ) { int32_T b_kstr ; char_T b_p [ 9 ] ; char_T b [ 8 ] ;
boolean_T b_bool ; static const char_T tmp [ 8 ] = { 'r' , 'e' , 'v' , 'o' ,
'l' , 'u' , 't' , 'e' } ; static const char_T tmp_p [ 9 ] = { 'p' , 'r' , 'i'
, 's' , 'm' , 'a' , 't' , 'i' , 'c' } ; int32_T exitg1 ; boolean_T guard1 =
false ; for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { b [ b_kstr ] = tmp [
b_kstr ] ; } b_bool = false ; if ( obj -> Type -> size [ 1 ] != 8 ) { } else
{ b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if ( obj -> Type ->
data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ;
} } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } guard1
= false ; if ( b_bool ) { guard1 = true ; } else { for ( b_kstr = 0 ; b_kstr
< 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_p [ b_kstr ] ; } if ( obj -> Type ->
size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 <
9 ) { if ( obj -> Type -> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) {
exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } }
while ( exitg1 == 0 ) ; } if ( b_bool ) { guard1 = true ; } else { ax [ 0 ] =
( rtNaN ) ; ax [ 1 ] = ( rtNaN ) ; ax [ 2 ] = ( rtNaN ) ; } } if ( guard1 ) {
ax [ 0 ] = obj -> JointAxisInternal [ 0 ] ; ax [ 1 ] = obj ->
JointAxisInternal [ 1 ] ; ax [ 2 ] = obj -> JointAxisInternal [ 2 ] ; } }
static void jhl11noygj ( hzpfvcreu1 * * pEmxArray ) { if ( * pEmxArray != (
hzpfvcreu1 * ) NULL ) { if ( ( ( * pEmxArray ) -> data != ( bkyb12klys * )
NULL ) && ( * pEmxArray ) -> canFreeData ) { free ( ( * pEmxArray ) -> data )
; } free ( ( * pEmxArray ) -> size ) ; free ( * pEmxArray ) ; * pEmxArray = (
hzpfvcreu1 * ) NULL ; } } static void cdtxuhnnn2 ( hzpfvcreu1o * * pEmxArray
, int32_T numDimensions ) { hzpfvcreu1o * emxArray ; int32_T i ; * pEmxArray
= ( hzpfvcreu1o * ) malloc ( sizeof ( hzpfvcreu1o ) ) ; emxArray = *
pEmxArray ; emxArray -> data = ( bkyb12klys3 * ) NULL ; emxArray ->
numDimensions = numDimensions ; emxArray -> size = ( int32_T * ) malloc (
sizeof ( int32_T ) * ( uint32_T ) numDimensions ) ; emxArray -> allocatedSize
= 0 ; emxArray -> canFreeData = true ; for ( i = 0 ; i < numDimensions ; i ++
) { emxArray -> size [ i ] = 0 ; } } static void obcr1nckmu ( hzpfvcreu1o *
emxArray , int32_T oldNumel ) { int32_T i ; int32_T newNumel ; void * newData
; if ( oldNumel < 0 ) { oldNumel = 0 ; } newNumel = 1 ; for ( i = 0 ; i <
emxArray -> numDimensions ; i ++ ) { newNumel *= emxArray -> size [ i ] ; }
if ( newNumel > emxArray -> allocatedSize ) { i = emxArray -> allocatedSize ;
if ( i < 16 ) { i = 16 ; } while ( i < newNumel ) { if ( i > 1073741823 ) { i
= MAX_int32_T ; } else { i <<= 1 ; } } newData = calloc ( ( uint32_T ) i ,
sizeof ( bkyb12klys3 ) ) ; if ( emxArray -> data != NULL ) { memcpy ( newData
, emxArray -> data , sizeof ( bkyb12klys3 ) * ( uint32_T ) oldNumel ) ; if (
emxArray -> canFreeData ) { free ( emxArray -> data ) ; } } emxArray -> data
= ( bkyb12klys3 * ) newData ; emxArray -> allocatedSize = i ; emxArray ->
canFreeData = true ; } } static void cnixr4pd3aci ( const ot4uovzkt0wp * obj
, real_T ax [ 3 ] ) { int32_T b_kstr ; char_T b_p [ 9 ] ; char_T b [ 8 ] ;
boolean_T b_bool ; static const char_T tmp [ 8 ] = { 'r' , 'e' , 'v' , 'o' ,
'l' , 'u' , 't' , 'e' } ; static const char_T tmp_p [ 9 ] = { 'p' , 'r' , 'i'
, 's' , 'm' , 'a' , 't' , 'i' , 'c' } ; int32_T exitg1 ; boolean_T guard1 =
false ; for ( b_kstr = 0 ; b_kstr < 8 ; b_kstr ++ ) { b [ b_kstr ] = tmp [
b_kstr ] ; } b_bool = false ; if ( obj -> Type -> size [ 1 ] != 8 ) { } else
{ b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 < 8 ) { if ( obj -> Type ->
data [ b_kstr - 1 ] != b [ b_kstr - 1 ] ) { exitg1 = 1 ; } else { b_kstr ++ ;
} } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } guard1
= false ; if ( b_bool ) { guard1 = true ; } else { for ( b_kstr = 0 ; b_kstr
< 9 ; b_kstr ++ ) { b_p [ b_kstr ] = tmp_p [ b_kstr ] ; } if ( obj -> Type ->
size [ 1 ] != 9 ) { } else { b_kstr = 1 ; do { exitg1 = 0 ; if ( b_kstr - 1 <
9 ) { if ( obj -> Type -> data [ b_kstr - 1 ] != b_p [ b_kstr - 1 ] ) {
exitg1 = 1 ; } else { b_kstr ++ ; } } else { b_bool = true ; exitg1 = 1 ; } }
while ( exitg1 == 0 ) ; } if ( b_bool ) { guard1 = true ; } else { ax [ 0 ] =
( rtNaN ) ; ax [ 1 ] = ( rtNaN ) ; ax [ 2 ] = ( rtNaN ) ; } } if ( guard1 ) {
ax [ 0 ] = obj -> JointAxisInternal [ 0 ] ; ax [ 1 ] = obj ->
JointAxisInternal [ 1 ] ; ax [ 2 ] = obj -> JointAxisInternal [ 2 ] ; } }
static void n4riw4ioyca ( const ot4uovzkt0wp * obj , const real_T q_data [ ]
, const int32_T * q_size , real_T T [ 16 ] ) { real_T b [ 16 ] ; real_T obj_p
[ 16 ] ; real_T R [ 9 ] ; real_T tempR [ 9 ] ; real_T result_data [ 4 ] ;
real_T v [ 3 ] ; real_T cth ; real_T theta ; int32_T i ; int32_T i_p ; char_T
b_e [ 8 ] ; char_T b_p [ 5 ] ; boolean_T b_bool ; static const char_T tmp [ 5
] = { 'f' , 'i' , 'x' , 'e' , 'd' } ; static const char_T tmp_p [ 8 ] = { 'r'
, 'e' , 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ; int32_T exitg1 ; for ( i = 0 ;
i < 5 ; i ++ ) { b_p [ i ] = tmp [ i ] ; } b_bool = false ; if ( obj -> Type
-> size [ 1 ] != 5 ) { } else { i = 1 ; do { exitg1 = 0 ; if ( i - 1 < 5 ) {
if ( obj -> Type -> data [ i - 1 ] != b_p [ i - 1 ] ) { exitg1 = 1 ; } else {
i ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; }
if ( b_bool ) { cth = 0.0 ; } else { for ( i = 0 ; i < 8 ; i ++ ) { b_e [ i ]
= tmp_p [ i ] ; } if ( obj -> Type -> size [ 1 ] != 8 ) { } else { i = 1 ; do
{ exitg1 = 0 ; if ( i - 1 < 8 ) { if ( obj -> Type -> data [ i - 1 ] != b_e [
i - 1 ] ) { exitg1 = 1 ; } else { i ++ ; } } else { b_bool = true ; exitg1 =
1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { cth = 1.0 ; } else { cth =
- 1.0 ; } } switch ( ( int32_T ) cth ) { case 0 : memset ( & b [ 0 ] , 0 ,
sizeof ( real_T ) << 4U ) ; b [ 0 ] = 1.0 ; b [ 5 ] = 1.0 ; b [ 10 ] = 1.0 ;
b [ 15 ] = 1.0 ; break ; case 1 : cnixr4pd3aci ( obj , v ) ; result_data [ 0
] = v [ 0 ] ; result_data [ 1 ] = v [ 1 ] ; result_data [ 2 ] = v [ 2 ] ; if
( ( * q_size != 0 ) - 1 >= 0 ) { result_data [ 3 ] = q_data [ 0 ] ; } cth =
result_data [ 0 ] ; v [ 0 ] = cth * cth ; cth = result_data [ 1 ] ; v [ 1 ] =
cth * cth ; cth = result_data [ 2 ] ; cth = 1.0 / muDoubleScalarSqrt ( ( v [
0 ] + v [ 1 ] ) + cth * cth ) ; v [ 0 ] = result_data [ 0 ] * cth ; v [ 1 ] =
result_data [ 1 ] * cth ; v [ 2 ] = result_data [ 2 ] * cth ; theta =
result_data [ 3 ] ; cth = muDoubleScalarCos ( theta ) ; theta =
muDoubleScalarSin ( theta ) ; lpd2uo4ns5 ( v [ 0 ] * v [ 0 ] * ( 1.0 - cth )
+ cth , v [ 0 ] * v [ 1 ] * ( 1.0 - cth ) - v [ 2 ] * theta , v [ 0 ] * v [ 2
] * ( 1.0 - cth ) + v [ 1 ] * theta , v [ 0 ] * v [ 1 ] * ( 1.0 - cth ) + v [
2 ] * theta , v [ 1 ] * v [ 1 ] * ( 1.0 - cth ) + cth , v [ 1 ] * v [ 2 ] * (
1.0 - cth ) - v [ 0 ] * theta , v [ 0 ] * v [ 2 ] * ( 1.0 - cth ) - v [ 1 ] *
theta , v [ 1 ] * v [ 2 ] * ( 1.0 - cth ) + v [ 0 ] * theta , v [ 2 ] * v [ 2
] * ( 1.0 - cth ) + cth , tempR ) ; for ( i = 0 ; i < 3 ; i ++ ) { R [ i ] =
tempR [ i * 3 ] ; R [ i + 3 ] = tempR [ i * 3 + 1 ] ; R [ i + 6 ] = tempR [ i
* 3 + 2 ] ; } memset ( & b [ 0 ] , 0 , sizeof ( real_T ) << 4U ) ; for ( i =
0 ; i < 3 ; i ++ ) { b [ i << 2 ] = R [ 3 * i ] ; b [ ( i << 2 ) + 1 ] = R [
3 * i + 1 ] ; b [ ( i << 2 ) + 2 ] = R [ 3 * i + 2 ] ; } b [ 15 ] = 1.0 ;
break ; default : cnixr4pd3aci ( obj , v ) ; memset ( & tempR [ 0 ] , 0 , 9U
* sizeof ( real_T ) ) ; tempR [ 0 ] = 1.0 ; tempR [ 4 ] = 1.0 ; tempR [ 8 ] =
1.0 ; cth = q_data [ 0 ] ; for ( i = 0 ; i < 3 ; i ++ ) { b [ i << 2 ] =
tempR [ 3 * i ] ; b [ ( i << 2 ) + 1 ] = tempR [ 3 * i + 1 ] ; b [ ( i << 2 )
+ 2 ] = tempR [ 3 * i + 2 ] ; b [ i + 12 ] = v [ i ] * cth ; } b [ 3 ] = 0.0
; b [ 7 ] = 0.0 ; b [ 11 ] = 0.0 ; b [ 15 ] = 1.0 ; break ; } for ( i = 0 ; i
< 4 ; i ++ ) { for ( i_p = 0 ; i_p < 4 ; i_p ++ ) { obj_p [ i + ( i_p << 2 )
] = 0.0 ; obj_p [ i + ( i_p << 2 ) ] += b [ i_p << 2 ] * obj ->
JointToParentTransform [ i ] ; obj_p [ i + ( i_p << 2 ) ] += b [ ( i_p << 2 )
+ 1 ] * obj -> JointToParentTransform [ i + 4 ] ; obj_p [ i + ( i_p << 2 ) ]
+= b [ ( i_p << 2 ) + 2 ] * obj -> JointToParentTransform [ i + 8 ] ; obj_p [
i + ( i_p << 2 ) ] += b [ ( i_p << 2 ) + 3 ] * obj -> JointToParentTransform
[ i + 12 ] ; } for ( i_p = 0 ; i_p < 4 ; i_p ++ ) { T [ i + ( i_p << 2 ) ] =
0.0 ; T [ i + ( i_p << 2 ) ] += obj -> ChildToJointTransform [ i_p << 2 ] *
obj_p [ i ] ; T [ i + ( i_p << 2 ) ] += obj -> ChildToJointTransform [ ( i_p
<< 2 ) + 1 ] * obj_p [ i + 4 ] ; T [ i + ( i_p << 2 ) ] += obj ->
ChildToJointTransform [ ( i_p << 2 ) + 2 ] * obj_p [ i + 8 ] ; T [ i + ( i_p
<< 2 ) ] += obj -> ChildToJointTransform [ ( i_p << 2 ) + 3 ] * obj_p [ i +
12 ] ; } } } static void n4riw4ioyc ( const ot4uovzkt0wp * obj , real_T T [
16 ] ) { real_T b [ 16 ] ; real_T obj_p [ 16 ] ; real_T R [ 9 ] ; real_T
tempR [ 9 ] ; real_T v [ 3 ] ; real_T axang_idx_0 ; real_T axang_idx_1 ;
real_T axang_idx_2 ; real_T b_index ; int32_T i ; int32_T i_p ; char_T b_e [
8 ] ; char_T b_p [ 5 ] ; boolean_T b_bool ; static const char_T tmp [ 5 ] = {
'f' , 'i' , 'x' , 'e' , 'd' } ; static const char_T tmp_p [ 8 ] = { 'r' , 'e'
, 'v' , 'o' , 'l' , 'u' , 't' , 'e' } ; int32_T exitg1 ; for ( i = 0 ; i < 5
; i ++ ) { b_p [ i ] = tmp [ i ] ; } b_bool = false ; if ( obj -> Type ->
size [ 1 ] != 5 ) { } else { i = 1 ; do { exitg1 = 0 ; if ( i - 1 < 5 ) { if
( obj -> Type -> data [ i - 1 ] != b_p [ i - 1 ] ) { exitg1 = 1 ; } else { i
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { b_index = 0.0 ; } else { for ( i = 0 ; i < 8 ; i ++ ) { b_e [ i
] = tmp_p [ i ] ; } if ( obj -> Type -> size [ 1 ] != 8 ) { } else { i = 1 ;
do { exitg1 = 0 ; if ( i - 1 < 8 ) { if ( obj -> Type -> data [ i - 1 ] !=
b_e [ i - 1 ] ) { exitg1 = 1 ; } else { i ++ ; } } else { b_bool = true ;
exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { b_index = 1.0 ; }
else { b_index = - 1.0 ; } } switch ( ( int32_T ) b_index ) { case 0 : memset
( & b [ 0 ] , 0 , sizeof ( real_T ) << 4U ) ; b [ 0 ] = 1.0 ; b [ 5 ] = 1.0 ;
b [ 10 ] = 1.0 ; b [ 15 ] = 1.0 ; break ; case 1 : cnixr4pd3aci ( obj , v ) ;
axang_idx_0 = v [ 0 ] ; axang_idx_1 = v [ 1 ] ; axang_idx_2 = v [ 2 ] ;
b_index = 1.0 / muDoubleScalarSqrt ( ( axang_idx_0 * axang_idx_0 +
axang_idx_1 * axang_idx_1 ) + axang_idx_2 * axang_idx_2 ) ; v [ 0 ] =
axang_idx_0 * b_index ; v [ 1 ] = axang_idx_1 * b_index ; v [ 2 ] =
axang_idx_2 * b_index ; lpd2uo4ns5 ( v [ 0 ] * v [ 0 ] * 0.0 + 1.0 , v [ 0 ]
* v [ 1 ] * 0.0 - v [ 2 ] * 0.0 , v [ 0 ] * v [ 2 ] * 0.0 + v [ 1 ] * 0.0 , v
[ 0 ] * v [ 1 ] * 0.0 + v [ 2 ] * 0.0 , v [ 1 ] * v [ 1 ] * 0.0 + 1.0 , v [ 1
] * v [ 2 ] * 0.0 - v [ 0 ] * 0.0 , v [ 0 ] * v [ 2 ] * 0.0 - v [ 1 ] * 0.0 ,
v [ 1 ] * v [ 2 ] * 0.0 + v [ 0 ] * 0.0 , v [ 2 ] * v [ 2 ] * 0.0 + 1.0 ,
tempR ) ; for ( i = 0 ; i < 3 ; i ++ ) { R [ i ] = tempR [ i * 3 ] ; R [ i +
3 ] = tempR [ i * 3 + 1 ] ; R [ i + 6 ] = tempR [ i * 3 + 2 ] ; } memset ( &
b [ 0 ] , 0 , sizeof ( real_T ) << 4U ) ; for ( i = 0 ; i < 3 ; i ++ ) { b [
i << 2 ] = R [ 3 * i ] ; b [ ( i << 2 ) + 1 ] = R [ 3 * i + 1 ] ; b [ ( i <<
2 ) + 2 ] = R [ 3 * i + 2 ] ; } b [ 15 ] = 1.0 ; break ; default :
cnixr4pd3aci ( obj , v ) ; memset ( & tempR [ 0 ] , 0 , 9U * sizeof ( real_T
) ) ; tempR [ 0 ] = 1.0 ; tempR [ 4 ] = 1.0 ; tempR [ 8 ] = 1.0 ; for ( i = 0
; i < 3 ; i ++ ) { b [ i << 2 ] = tempR [ 3 * i ] ; b [ ( i << 2 ) + 1 ] =
tempR [ 3 * i + 1 ] ; b [ ( i << 2 ) + 2 ] = tempR [ 3 * i + 2 ] ; b [ i + 12
] = v [ i ] * 0.0 ; } b [ 3 ] = 0.0 ; b [ 7 ] = 0.0 ; b [ 11 ] = 0.0 ; b [ 15
] = 1.0 ; break ; } for ( i = 0 ; i < 4 ; i ++ ) { for ( i_p = 0 ; i_p < 4 ;
i_p ++ ) { obj_p [ i + ( i_p << 2 ) ] = 0.0 ; obj_p [ i + ( i_p << 2 ) ] += b
[ i_p << 2 ] * obj -> JointToParentTransform [ i ] ; obj_p [ i + ( i_p << 2 )
] += b [ ( i_p << 2 ) + 1 ] * obj -> JointToParentTransform [ i + 4 ] ; obj_p
[ i + ( i_p << 2 ) ] += b [ ( i_p << 2 ) + 2 ] * obj ->
JointToParentTransform [ i + 8 ] ; obj_p [ i + ( i_p << 2 ) ] += b [ ( i_p <<
2 ) + 3 ] * obj -> JointToParentTransform [ i + 12 ] ; } for ( i_p = 0 ; i_p
< 4 ; i_p ++ ) { T [ i + ( i_p << 2 ) ] = 0.0 ; T [ i + ( i_p << 2 ) ] += obj
-> ChildToJointTransform [ i_p << 2 ] * obj_p [ i ] ; T [ i + ( i_p << 2 ) ]
+= obj -> ChildToJointTransform [ ( i_p << 2 ) + 1 ] * obj_p [ i + 4 ] ; T [
i + ( i_p << 2 ) ] += obj -> ChildToJointTransform [ ( i_p << 2 ) + 2 ] *
obj_p [ i + 8 ] ; T [ i + ( i_p << 2 ) ] += obj -> ChildToJointTransform [ (
i_p << 2 ) + 3 ] * obj_p [ i + 12 ] ; } } } static void oxlwy3eg3q (
hzpfvcreu1o * * pEmxArray ) { if ( * pEmxArray != ( hzpfvcreu1o * ) NULL ) {
if ( ( ( * pEmxArray ) -> data != ( bkyb12klys3 * ) NULL ) && ( * pEmxArray )
-> canFreeData ) { free ( ( * pEmxArray ) -> data ) ; } free ( ( * pEmxArray
) -> size ) ; free ( * pEmxArray ) ; * pEmxArray = ( hzpfvcreu1o * ) NULL ; }
} static void j0alujw22n ( alyuhn31fbfz * pStruct ) { fagcrr3avn ( & pStruct
-> NameInternal ) ; } static void j0alujw22n13 ( ogx4llc4arl * pStruct ) {
oamrpiq5ob ( & pStruct -> CollisionGeometries ) ; } static void ovdvrmzlss (
ogx4llc4arl pMatrix [ 19 ] ) { int32_T i ; for ( i = 0 ; i < 19 ; i ++ ) {
j0alujw22n13 ( & pMatrix [ i ] ) ; } } static void jfnfdyiidb ( ot4uovzkt0w *
pStruct ) { fagcrr3avn ( & pStruct -> Type ) ; bdhoypqe5x ( & pStruct ->
MotionSubspace ) ; fagcrr3avn ( & pStruct -> NameInternal ) ; bdhoypqe5x ( &
pStruct -> PositionLimitsInternal ) ; bdhoypqe5x ( & pStruct ->
HomePositionInternal ) ; } static void dqwgmpsyro ( ot4uovzkt0w pMatrix [ 19
] ) { int32_T i ; for ( i = 0 ; i < 19 ; i ++ ) { jfnfdyiidb ( & pMatrix [ i
] ) ; } } static void ovdvrmzlss0 ( alyuhn31fbfz pMatrix [ 18 ] ) { int32_T i
; for ( i = 0 ; i < 18 ; i ++ ) { j0alujw22n ( & pMatrix [ i ] ) ; } } static
void j0alujw22n1 ( mjv2tzlyyigm * pStruct ) { j0alujw22n ( & pStruct -> Base
) ; ovdvrmzlss ( pStruct -> _pobj0 ) ; dqwgmpsyro ( pStruct -> _pobj1 ) ;
ovdvrmzlss0 ( pStruct -> _pobj2 ) ; } static void j0alujw22n13v ( lnin0tndd2
* pStruct ) { fagcrr3avn ( & pStruct -> BodyName ) ; bdhoypqe5x ( & pStruct
-> ErrTemp ) ; bdhoypqe5x ( & pStruct -> GradTemp ) ; } static void
ojga4a1v1p ( ot4uovzkt0w pMatrix [ 18 ] ) { int32_T i ; for ( i = 0 ; i < 18
; i ++ ) { jfnfdyiidb ( & pMatrix [ i ] ) ; } } static void ovdvrmzlss0z (
alyuhn31fbfz pMatrix [ 9 ] ) { int32_T i ; for ( i = 0 ; i < 9 ; i ++ ) {
j0alujw22n ( & pMatrix [ i ] ) ; } } static void ovdvrmzlss0z4 ( ogx4llc4arl
pMatrix [ 10 ] ) { int32_T i ; for ( i = 0 ; i < 10 ; i ++ ) { j0alujw22n13 (
& pMatrix [ i ] ) ; } } static void em2zaf1fxr ( ot4uovzkt0w pMatrix [ 10 ] )
{ int32_T i ; for ( i = 0 ; i < 10 ; i ++ ) { jfnfdyiidb ( & pMatrix [ i ] )
; } } static void j0alujw22n13v5 ( m0caqfktx0 * pStruct ) { j0alujw22n ( &
pStruct -> Base ) ; ovdvrmzlss0z ( pStruct -> _pobj0 ) ; ovdvrmzlss0z4 (
pStruct -> _pobj1 ) ; em2zaf1fxr ( pStruct -> _pobj2 ) ; } static void
n5jki5o22u ( ecfe5we0wc * pStruct ) { bdhoypqe5x ( & pStruct ->
ConstraintMatrix ) ; bdhoypqe5x ( & pStruct -> ConstraintBound ) ; } static
void b15a2b3iyt ( carpjyrt23 * pStruct ) { bdhoypqe5x ( & pStruct -> Limits )
; j0alujw22n13v ( & pStruct -> _pobj0 ) ; ojga4a1v1p ( pStruct -> _pobj1 ) ;
ovdvrmzlss0z ( pStruct -> _pobj2 ) ; ovdvrmzlss ( pStruct -> _pobj3 ) ;
j0alujw22n13v5 ( & pStruct -> _pobj4 ) ; n5jki5o22u ( & pStruct -> _pobj5 ) ;
} static void omslekmpnx ( ecbwrsiemx * pStruct ) { j0alujw22n1 ( & pStruct
-> TreeInternal ) ; b15a2b3iyt ( & pStruct -> IKInternal ) ; } static void
eq1frudh3p ( ot4uovzkt0 * pStruct ) { fagcrr3avn ( & pStruct -> Type ) ; }
static void fhh2u1a23n ( ogx4llc4ar * pStruct ) { oamrpiq5obm ( & pStruct ->
CollisionGeometries ) ; } static void j0alujw22n13v5u ( alyuhn31fb * pStruct
) { fagcrr3avn ( & pStruct -> NameInternal ) ; eq1frudh3p ( & pStruct ->
JointInternal ) ; fhh2u1a23n ( & pStruct -> CollisionsInternal ) ; } static
void ovdvrmzlss0z4t ( alyuhn31fb pMatrix [ 18 ] ) { int32_T i ; for ( i = 0 ;
i < 18 ; i ++ ) { j0alujw22n13v5u ( & pMatrix [ i ] ) ; } } static void
bfznjjiplu ( mjv2tzlyyi * pStruct ) { j0alujw22n13v5u ( & pStruct -> Base ) ;
ovdvrmzlss0z4t ( pStruct -> _pobj0 ) ; } static void omslekmpnxk ( evzhsot2uw
* pStruct ) { bfznjjiplu ( & pStruct -> TreeInternal ) ; } static void
k1aewmoyxc ( ot4uovzkt0wp * pStruct ) { fagcrr3avn ( & pStruct -> Type ) ;
bdhoypqe5x ( & pStruct -> MotionSubspace ) ; } static void ergoqrpdey (
alyuhn31fbf * pStruct ) { fagcrr3avn ( & pStruct -> NameInternal ) ;
k1aewmoyxc ( & pStruct -> JointInternal ) ; fhh2u1a23n ( & pStruct ->
CollisionsInternal ) ; } static void ovdvrmzlss0z4tv ( alyuhn31fbf pMatrix [
18 ] ) { int32_T i ; for ( i = 0 ; i < 18 ; i ++ ) { ergoqrpdey ( & pMatrix [
i ] ) ; } } static void e3vs32ulkf ( mjv2tzlyyig * pStruct ) { ergoqrpdey ( &
pStruct -> Base ) ; ovdvrmzlss0z4tv ( pStruct -> _pobj0 ) ; } static void
omslekmpnxkp ( jd4q4eqa1g * pStruct ) { e3vs32ulkf ( & pStruct ->
TreeInternal ) ; } void MdlInitialize ( void ) { int32_T i ; int_T tmp_e ;
int_T tmp_g ; int_T tmp_i ; int_T tmp_j ; int_T tmp_m ; boolean_T tmp ;
boolean_T tmp_p ; SimStruct * S ; void * diag ; rtDW . ahio0mgx5h [ 0 ] = rtP
. InputFilter_InitialStates ; rtDW . bwwbiride4 [ 0 ] = rtP .
InputRateLimiter_IC ; rtDW . ahio0mgx5h [ 1 ] = rtP .
InputFilter_InitialStates ; rtDW . bwwbiride4 [ 1 ] = rtP .
InputRateLimiter_IC ; rtDW . ahio0mgx5h [ 2 ] = rtP .
InputFilter_InitialStates ; rtDW . bwwbiride4 [ 2 ] = rtP .
InputRateLimiter_IC ; rtDW . ahio0mgx5h [ 3 ] = rtP .
InputFilter_InitialStates ; rtDW . bwwbiride4 [ 3 ] = rtP .
InputRateLimiter_IC ; rtDW . jmlpsyjpqa = rtP .
DiscretePIDController_InitialConditionForFilter ; rtDW . osgjdwjesl = rtP .
DiscretePIDController1_InitialConditionForFilter ; rtDW . fkcgzk5oma = rtP .
DiscretePIDController2_InitialConditionForFilter ; rtDW . c45aidngho = rtP .
DiscretePIDController3_InitialConditionForFilter ; rtDW . kaentdmtdn = rtP .
DiscretePIDController4_InitialConditionForIntegrator ; rtDW . d5wogx3kqa =
rtP . DiscretePIDController5_InitialConditionForIntegrator ; for ( i = 0 ; i
< 6 ; i ++ ) { rtDW . g4g3vrgle4 [ i ] = rtP .
RateTransition6_InitialCondition ; rtDW . jiqh55j12p [ i ] = rtP .
OutputFilter_InitialStates ; rtDW . ggzlanf4wa [ i ] = rtP .
DiscreteDerivative_ICPrevScaledInput ; rtDW . p4ux5yfv5l [ i ] = rtP .
DiscreteDerivative1_ICPrevScaledInput ; rtDW . ap5zbvosy0 [ i ] = rtP .
RateTransition_InitialCondition ; } tmp = false ; tmp_p = false ; if ( tmp_p
|| tmp ) { i = strcmp ( ssGetSolverName ( rtS ) , "daessc" ) ; tmp_e = strcmp
( ssGetSolverName ( rtS ) , "ode14x" ) ; tmp_i = strcmp ( ssGetSolverName (
rtS ) , "ode15s" ) ; tmp_m = strcmp ( ssGetSolverName ( rtS ) , "ode1be" ) ;
tmp_g = strcmp ( ssGetSolverName ( rtS ) , "ode23t" ) ; tmp_j = strcmp (
ssGetSolverName ( rtS ) , "odeN" ) ; if ( ( boolean_T ) ( ( i != 0 ) & (
tmp_e != 0 ) & ( tmp_i != 0 ) & ( tmp_m != 0 ) & ( tmp_g != 0 ) & ( tmp_j !=
0 ) ) ) { S = rtS ; diag = CreateDiagnosticAsVoidPtr (
 "physmod:simscape:engine:sli:SimscapeExecutionBlock:InconsistentSolversInModelRef"
, 2 , 3 , "daessc" , 3 , "{daessc, ode14x, ode15s, ode1be, ode23t, odeN}" ) ;
rt_ssSet_slErrMsg ( S , diag ) ; } } tmp = false ; tmp_p = false ; if ( tmp_p
|| tmp ) { i = strcmp ( ssGetSolverName ( rtS ) , "daessc" ) ; tmp_e = strcmp
( ssGetSolverName ( rtS ) , "ode14x" ) ; tmp_i = strcmp ( ssGetSolverName (
rtS ) , "ode15s" ) ; tmp_m = strcmp ( ssGetSolverName ( rtS ) , "ode1be" ) ;
tmp_g = strcmp ( ssGetSolverName ( rtS ) , "ode23t" ) ; tmp_j = strcmp (
ssGetSolverName ( rtS ) , "odeN" ) ; if ( ( boolean_T ) ( ( i != 0 ) & (
tmp_e != 0 ) & ( tmp_i != 0 ) & ( tmp_m != 0 ) & ( tmp_g != 0 ) & ( tmp_j !=
0 ) ) ) { S = rtS ; diag = CreateDiagnosticAsVoidPtr (
 "physmod:simscape:engine:sli:SimscapeExecutionBlock:InconsistentSolversInModelRef"
, 2 , 3 , "daessc" , 3 , "{daessc, ode14x, ode15s, ode1be, ode23t, odeN}" ) ;
rt_ssSet_slErrMsg ( S , diag ) ; } } { static int_T modelMassMatrixIr [ 14 ]
= { 0 , 1 , 2 , 3 , 4 , 5 , 6 , 7 , 8 , 9 , 10 , 11 , 12 , 14 } ; static
int_T modelMassMatrixJc [ 17 ] = { 0 , 1 , 2 , 3 , 4 , 5 , 6 , 7 , 8 , 9 , 10
, 11 , 12 , 13 , 13 , 14 , 14 } ; static real_T modelMassMatrixPr [ 14 ] = {
1.0 , 1.0 , 1.0 , 1.0 , 1.0 , 1.0 , 1.0 , 1.0 , 1.0 , 1.0 , 1.0 , 1.0 , 1.0 ,
1.0 } ; ( void ) memcpy ( rtMassMatrix . ir , modelMassMatrixIr , 14 * sizeof
( int_T ) ) ; ( void ) memcpy ( rtMassMatrix . jc , modelMassMatrixJc , 17 *
sizeof ( int_T ) ) ; ( void ) memcpy ( rtMassMatrix . pr , modelMassMatrixPr
, 14 * sizeof ( real_T ) ) ; } } void MdlStart ( void ) { NeModelParameters
modelParameters ; NeModelParameters modelParameters_e ; NeModelParameters
modelParameters_g ; NeModelParameters modelParameters_i ; NeModelParameters
modelParameters_m ; NeModelParameters modelParameters_p ; NeslSimulationData
* simulationData ; NeslSimulator * tmp ; NeuDiagnosticManager *
diagnosticManager ; NeuDiagnosticTree * diagnosticTree ; NeuDiagnosticTree *
diagnosticTree_c ; NeuDiagnosticTree * diagnosticTree_e ; NeuDiagnosticTree *
diagnosticTree_f ; NeuDiagnosticTree * diagnosticTree_g ; NeuDiagnosticTree *
diagnosticTree_i ; NeuDiagnosticTree * diagnosticTree_j ; NeuDiagnosticTree *
diagnosticTree_m ; NeuDiagnosticTree * diagnosticTree_p ; char * msg ; char *
msg_c ; char * msg_e ; char * msg_f ; char * msg_g ; char * msg_i ; char *
msg_j ; char * msg_m ; char * msg_p ; real_T tmp_i [ 40 ] ; real_T tmp_f [ 4
] ; real_T tmp_g [ 4 ] ; real_T time ; real_T time_e ; real_T time_p ; real_T
tmp_e ; int32_T i ; int_T tmp_m [ 11 ] ; int_T tmp_c [ 2 ] ; int_T tmp_j [ 2
] ; boolean_T tmp_p ; boolean_T val ; static const uint32_T tmp_k [ 625 ] = {
5489U , 1301868182U , 2938499221U , 2950281878U , 1875628136U , 751856242U ,
944701696U , 2243192071U , 694061057U , 219885934U , 2066767472U ,
3182869408U , 485472502U , 2336857883U , 1071588843U , 3418470598U ,
951210697U , 3693558366U , 2923482051U , 1793174584U , 2982310801U ,
1586906132U , 1951078751U , 1808158765U , 1733897588U , 431328322U ,
4202539044U , 530658942U , 1714810322U , 3025256284U , 3342585396U ,
1937033938U , 2640572511U , 1654299090U , 3692403553U , 4233871309U ,
3497650794U , 862629010U , 2943236032U , 2426458545U , 1603307207U ,
1133453895U , 3099196360U , 2208657629U , 2747653927U , 931059398U ,
761573964U , 3157853227U , 785880413U , 730313442U , 124945756U , 2937117055U
, 3295982469U , 1724353043U , 3021675344U , 3884886417U , 4010150098U ,
4056961966U , 699635835U , 2681338818U , 1339167484U , 720757518U ,
2800161476U , 2376097373U , 1532957371U , 3902664099U , 1238982754U ,
3725394514U , 3449176889U , 3570962471U , 4287636090U , 4087307012U ,
3603343627U , 202242161U , 2995682783U , 1620962684U , 3704723357U ,
371613603U , 2814834333U , 2111005706U , 624778151U , 2094172212U ,
4284947003U , 1211977835U , 991917094U , 1570449747U , 2962370480U ,
1259410321U , 170182696U , 146300961U , 2836829791U , 619452428U ,
2723670296U , 1881399711U , 1161269684U , 1675188680U , 4132175277U ,
780088327U , 3409462821U , 1036518241U , 1834958505U , 3048448173U ,
161811569U , 618488316U , 44795092U , 3918322701U , 1924681712U , 3239478144U
, 383254043U , 4042306580U , 2146983041U , 3992780527U , 3518029708U ,
3545545436U , 3901231469U , 1896136409U , 2028528556U , 2339662006U ,
501326714U , 2060962201U , 2502746480U , 561575027U , 581893337U ,
3393774360U , 1778912547U , 3626131687U , 2175155826U , 319853231U ,
986875531U , 819755096U , 2915734330U , 2688355739U , 3482074849U , 2736559U
, 2296975761U , 1029741190U , 2876812646U , 690154749U , 579200347U ,
4027461746U , 1285330465U , 2701024045U , 4117700889U , 759495121U ,
3332270341U , 2313004527U , 2277067795U , 4131855432U , 2722057515U ,
1264804546U , 3848622725U , 2211267957U , 4100593547U , 959123777U ,
2130745407U , 3194437393U , 486673947U , 1377371204U , 17472727U , 352317554U
, 3955548058U , 159652094U , 1232063192U , 3835177280U , 49423123U ,
3083993636U , 733092U , 2120519771U , 2573409834U , 1112952433U , 3239502554U
, 761045320U , 1087580692U , 2540165110U , 641058802U , 1792435497U ,
2261799288U , 1579184083U , 627146892U , 2165744623U , 2200142389U ,
2167590760U , 2381418376U , 1793358889U , 3081659520U , 1663384067U ,
2009658756U , 2689600308U , 739136266U , 2304581039U , 3529067263U ,
591360555U , 525209271U , 3131882996U , 294230224U , 2076220115U ,
3113580446U , 1245621585U , 1386885462U , 3203270426U , 123512128U ,
12350217U , 354956375U , 4282398238U , 3356876605U , 3888857667U , 157639694U
, 2616064085U , 1563068963U , 2762125883U , 4045394511U , 4180452559U ,
3294769488U , 1684529556U , 1002945951U , 3181438866U , 22506664U ,
691783457U , 2685221343U , 171579916U , 3878728600U , 2475806724U ,
2030324028U , 3331164912U , 1708711359U , 1970023127U , 2859691344U ,
2588476477U , 2748146879U , 136111222U , 2967685492U , 909517429U ,
2835297809U , 3206906216U , 3186870716U , 341264097U , 2542035121U ,
3353277068U , 548223577U , 3170936588U , 1678403446U , 297435620U ,
2337555430U , 466603495U , 1132321815U , 1208589219U , 696392160U ,
894244439U , 2562678859U , 470224582U , 3306867480U , 201364898U ,
2075966438U , 1767227936U , 2929737987U , 3674877796U , 2654196643U ,
3692734598U , 3528895099U , 2796780123U , 3048728353U , 842329300U ,
191554730U , 2922459673U , 3489020079U , 3979110629U , 1022523848U ,
2202932467U , 3583655201U , 3565113719U , 587085778U , 4176046313U ,
3013713762U , 950944241U , 396426791U , 3784844662U , 3477431613U ,
3594592395U , 2782043838U , 3392093507U , 3106564952U , 2829419931U ,
1358665591U , 2206918825U , 3170783123U , 31522386U , 2988194168U ,
1782249537U , 1105080928U , 843500134U , 1225290080U , 1521001832U ,
3605886097U , 2802786495U , 2728923319U , 3996284304U , 903417639U ,
1171249804U , 1020374987U , 2824535874U , 423621996U , 1988534473U ,
2493544470U , 1008604435U , 1756003503U , 1488867287U , 1386808992U ,
732088248U , 1780630732U , 2482101014U , 976561178U , 1543448953U ,
2602866064U , 2021139923U , 1952599828U , 2360242564U , 2117959962U ,
2753061860U , 2388623612U , 4138193781U , 2962920654U , 2284970429U ,
766920861U , 3457264692U , 2879611383U , 815055854U , 2332929068U ,
1254853997U , 3740375268U , 3799380844U , 4091048725U , 2006331129U ,
1982546212U , 686850534U , 1907447564U , 2682801776U , 2780821066U ,
998290361U , 1342433871U , 4195430425U , 607905174U , 3902331779U ,
2454067926U , 1708133115U , 1170874362U , 2008609376U , 3260320415U ,
2211196135U , 433538229U , 2728786374U , 2189520818U , 262554063U ,
1182318347U , 3710237267U , 1221022450U , 715966018U , 2417068910U ,
2591870721U , 2870691989U , 3418190842U , 4238214053U , 1540704231U ,
1575580968U , 2095917976U , 4078310857U , 2313532447U , 2110690783U ,
4056346629U , 4061784526U , 1123218514U , 551538993U , 597148360U ,
4120175196U , 3581618160U , 3181170517U , 422862282U , 3227524138U ,
1713114790U , 662317149U , 1230418732U , 928171837U , 1324564878U ,
1928816105U , 1786535431U , 2878099422U , 3290185549U , 539474248U ,
1657512683U , 552370646U , 1671741683U , 3655312128U , 1552739510U ,
2605208763U , 1441755014U , 181878989U , 3124053868U , 1447103986U ,
3183906156U , 1728556020U , 3502241336U , 3055466967U , 1013272474U ,
818402132U , 1715099063U , 2900113506U , 397254517U , 4194863039U ,
1009068739U , 232864647U , 2540223708U , 2608288560U , 2415367765U ,
478404847U , 3455100648U , 3182600021U , 2115988978U , 434269567U ,
4117179324U , 3461774077U , 887256537U , 3545801025U , 286388911U ,
3451742129U , 1981164769U , 786667016U , 3310123729U , 3097811076U ,
2224235657U , 2959658883U , 3370969234U , 2514770915U , 3345656436U ,
2677010851U , 2206236470U , 271648054U , 2342188545U , 4292848611U ,
3646533909U , 3754009956U , 3803931226U , 4160647125U , 1477814055U ,
4043852216U , 1876372354U , 3133294443U , 3871104810U , 3177020907U ,
2074304428U , 3479393793U , 759562891U , 164128153U , 1839069216U ,
2114162633U , 3989947309U , 3611054956U , 1333547922U , 835429831U ,
494987340U , 171987910U , 1252001001U , 370809172U , 3508925425U ,
2535703112U , 1276855041U , 1922855120U , 835673414U , 3030664304U ,
613287117U , 171219893U , 3423096126U , 3376881639U , 2287770315U ,
1658692645U , 1262815245U , 3957234326U , 1168096164U , 2968737525U ,
2655813712U , 2132313144U , 3976047964U , 326516571U , 353088456U ,
3679188938U , 3205649712U , 2654036126U , 1249024881U , 880166166U ,
691800469U , 2229503665U , 1673458056U , 4032208375U , 1851778863U ,
2563757330U , 376742205U , 1794655231U , 340247333U , 1505873033U ,
396524441U , 879666767U , 3335579166U , 3260764261U , 3335999539U ,
506221798U , 4214658741U , 975887814U , 2080536343U , 3360539560U ,
571586418U , 138896374U , 4234352651U , 2737620262U , 3928362291U ,
1516365296U , 38056726U , 3599462320U , 3585007266U , 3850961033U ,
471667319U , 1536883193U , 2310166751U , 1861637689U , 2530999841U ,
4139843801U , 2710569485U , 827578615U , 2012334720U , 2907369459U ,
3029312804U , 2820112398U , 1965028045U , 35518606U , 2478379033U ,
643747771U , 1924139484U , 4123405127U , 3811735531U , 3429660832U ,
3285177704U , 1948416081U , 1311525291U , 1183517742U , 1739192232U ,
3979815115U , 2567840007U , 4116821529U , 213304419U , 4125718577U ,
1473064925U , 2442436592U , 1893310111U , 4195361916U , 3747569474U ,
828465101U , 2991227658U , 750582866U , 1205170309U , 1409813056U ,
678418130U , 1171531016U , 3821236156U , 354504587U , 4202874632U ,
3882511497U , 1893248677U , 1903078632U , 26340130U , 2069166240U ,
3657122492U , 3725758099U , 831344905U , 811453383U , 3447711422U ,
2434543565U , 4166886888U , 3358210805U , 4142984013U , 2988152326U ,
3527824853U , 982082992U , 2809155763U , 190157081U , 3340214818U ,
2365432395U , 2548636180U , 2894533366U , 3474657421U , 2372634704U ,
2845748389U , 43024175U , 2774226648U , 1987702864U , 3186502468U ,
453610222U , 4204736567U , 1392892630U , 2471323686U , 2470534280U ,
3541393095U , 4269885866U , 3909911300U , 759132955U , 1482612480U ,
667715263U , 1795580598U , 2337923983U , 3390586366U , 581426223U ,
1515718634U , 476374295U , 705213300U , 363062054U , 2084697697U ,
2407503428U , 2292957699U , 2426213835U , 2199989172U , 1987356470U ,
4026755612U , 2147252133U , 270400031U , 1367820199U , 2369854699U ,
2844269403U , 79981964U , 624U } ; CXPtMax * _rtXPerturbMax ; CXPtMin *
_rtXPerturbMin ; { bool externalInputIsInDatasetFormat = false ; void *
pISigstreamManager = rt_GetISigstreamManager ( rtS ) ;
rtwISigstreamManagerGetInputIsInDatasetFormat ( pISigstreamManager , &
externalInputIsInDatasetFormat ) ; if ( externalInputIsInDatasetFormat ) { }
} _rtXPerturbMax = ( ( CXPtMax * ) ssGetJacobianPerturbationBoundsMaxVec (
rtS ) ) ; _rtXPerturbMin = ( ( CXPtMin * )
ssGetJacobianPerturbationBoundsMinVec ( rtS ) ) ; { { { bool
isStreamoutAlreadyRegistered = false ; { sdiSignalSourceInfoU srcInfo ;
sdiLabelU loggedName = sdiGetLabelFromChars ( "eePos" ) ; sdiLabelU
origSigName = sdiGetLabelFromChars ( "eePos" ) ; sdiLabelU propName =
sdiGetLabelFromChars ( "" ) ; sdiLabelU blockPath = sdiGetLabelFromChars (
"openManipulatorTrajectoryPlanning/Forward Kinematics" ) ; sdiLabelU blockSID
= sdiGetLabelFromChars ( "" ) ; sdiLabelU subPath = sdiGetLabelFromChars ( ""
) ; sdiDims sigDims ; sdiLabelU sigName = sdiGetLabelFromChars ( "eePos" ) ;
sdiAsyncRepoDataTypeHandle hDT = sdiAsyncRepoGetBuiltInDataTypeHandle (
DATA_TYPE_DOUBLE ) ; { sdiComplexity sigComplexity = REAL ;
sdiSampleTimeContinuity stCont = SAMPLE_TIME_DISCRETE ; int_T sigDimsArray [
2 ] = { 3 , 1 } ; sigDims . nDims = 2 ; sigDims . dimensions = sigDimsArray ;
srcInfo . numBlockPathElems = 1 ; srcInfo . fullBlockPath = ( sdiFullBlkPathU
) & blockPath ; srcInfo . SID = ( sdiSignalIDU ) & blockSID ; srcInfo .
subPath = subPath ; srcInfo . portIndex = 0 + 1 ; srcInfo . signalName =
sigName ; srcInfo . sigSourceUUID = 0 ; rtDW . mdhjjuxljs . AQHandles =
sdiStartAsyncioQueueCreation ( hDT , & srcInfo , rt_dataMapInfo . mmi .
InstanceMap . fullPath , "4ae9c957-ad86-4f10-b03d-00927d1b56e8" ,
sigComplexity , & sigDims , DIMENSIONS_MODE_FIXED , stCont , "" ) ;
sdiCompleteAsyncioQueueCreation ( rtDW . mdhjjuxljs . AQHandles , hDT , &
srcInfo ) ; if ( rtDW . mdhjjuxljs . AQHandles ) {
sdiSetSignalSampleTimeString ( rtDW . mdhjjuxljs . AQHandles ,
"&#x8FDE;&#x7EED;" , 0.0 , ssGetTFinal ( rtS ) ) ; sdiSetSignalRefRate ( rtDW
. mdhjjuxljs . AQHandles , 0.0 ) ; sdiSetRunStartTime ( rtDW . mdhjjuxljs .
AQHandles , ssGetTaskTime ( rtS , 1 ) ) ; sdiAsyncRepoSetSignalExportSettings
( rtDW . mdhjjuxljs . AQHandles , 1 , 0 ) ; sdiAsyncRepoSetSignalExportName (
rtDW . mdhjjuxljs . AQHandles , loggedName , origSigName , propName ) ; }
sdiFreeLabel ( sigName ) ; sdiFreeLabel ( loggedName ) ; sdiFreeLabel (
origSigName ) ; sdiFreeLabel ( propName ) ; sdiFreeLabel ( blockPath ) ;
sdiFreeLabel ( blockSID ) ; sdiFreeLabel ( subPath ) ; } } if ( !
isStreamoutAlreadyRegistered ) { } } } } { { { bool
isStreamoutAlreadyRegistered = false ; { sdiSignalSourceInfoU srcInfo ;
sdiLabelU loggedName = sdiGetLabelFromChars ( "IK Iterations" ) ; sdiLabelU
origSigName = sdiGetLabelFromChars ( "IK Iterations" ) ; sdiLabelU propName =
sdiGetLabelFromChars ( "" ) ; sdiLabelU blockPath = sdiGetLabelFromChars (
"openManipulatorTrajectoryPlanning/Inverse Kinematics" ) ; sdiLabelU blockSID
= sdiGetLabelFromChars ( "" ) ; sdiLabelU subPath = sdiGetLabelFromChars ( ""
) ; sdiDims sigDims ; sdiLabelU sigName = sdiGetLabelFromChars (
"IK Iterations" ) ; sdiAsyncRepoDataTypeHandle hDT =
sdiAsyncRepoGetBuiltInDataTypeHandle ( DATA_TYPE_DOUBLE ) ; { sdiComplexity
sigComplexity = REAL ; sdiSampleTimeContinuity stCont =
SAMPLE_TIME_CONTINUOUS ; int_T sigDimsArray [ 2 ] = { 1 , 1 } ; sigDims .
nDims = 2 ; sigDims . dimensions = sigDimsArray ; srcInfo . numBlockPathElems
= 1 ; srcInfo . fullBlockPath = ( sdiFullBlkPathU ) & blockPath ; srcInfo .
SID = ( sdiSignalIDU ) & blockSID ; srcInfo . subPath = subPath ; srcInfo .
portIndex = 0 + 1 ; srcInfo . signalName = sigName ; srcInfo . sigSourceUUID
= 0 ; rtDW . h42mla3rja . AQHandles = sdiStartAsyncioQueueCreation ( hDT , &
srcInfo , rt_dataMapInfo . mmi . InstanceMap . fullPath ,
"ea0d76dd-fbb8-4d84-8283-2be8a750b049" , sigComplexity , & sigDims ,
DIMENSIONS_MODE_FIXED , stCont , "" ) ; sdiCompleteAsyncioQueueCreation (
rtDW . h42mla3rja . AQHandles , hDT , & srcInfo ) ; if ( rtDW . h42mla3rja .
AQHandles ) { sdiSetSignalSampleTimeString ( rtDW . h42mla3rja . AQHandles ,
"&#x8FDE;&#x7EED;" , 0.0 , ssGetTFinal ( rtS ) ) ; sdiSetSignalRefRate ( rtDW
. h42mla3rja . AQHandles , 0.0 ) ; sdiSetRunStartTime ( rtDW . h42mla3rja .
AQHandles , ssGetTaskTime ( rtS , 1 ) ) ; sdiAsyncRepoSetSignalExportSettings
( rtDW . h42mla3rja . AQHandles , 1 , 0 ) ; sdiAsyncRepoSetSignalExportName (
rtDW . h42mla3rja . AQHandles , loggedName , origSigName , propName ) ; }
sdiFreeLabel ( sigName ) ; sdiFreeLabel ( loggedName ) ; sdiFreeLabel (
origSigName ) ; sdiFreeLabel ( propName ) ; sdiFreeLabel ( blockPath ) ;
sdiFreeLabel ( blockSID ) ; sdiFreeLabel ( subPath ) ; } } if ( !
isStreamoutAlreadyRegistered ) { } } } } { { { bool
isStreamoutAlreadyRegistered = false ; { sdiSignalSourceInfoU srcInfo ;
sdiLabelU loggedName = sdiGetLabelFromChars ( "jointPos" ) ; sdiLabelU
origSigName = sdiGetLabelFromChars ( "jointPos" ) ; sdiLabelU propName =
sdiGetLabelFromChars ( "" ) ; sdiLabelU blockPath = sdiGetLabelFromChars (
"openManipulatorTrajectoryPlanning/Rate Transition4" ) ; sdiLabelU blockSID =
sdiGetLabelFromChars ( "" ) ; sdiLabelU subPath = sdiGetLabelFromChars ( "" )
; sdiDims sigDims ; sdiLabelU sigName = sdiGetLabelFromChars ( "jointPos" ) ;
sdiAsyncRepoDataTypeHandle hDT = sdiAsyncRepoGetBuiltInDataTypeHandle (
DATA_TYPE_DOUBLE ) ; { sdiComplexity sigComplexity = REAL ;
sdiSampleTimeContinuity stCont = SAMPLE_TIME_DISCRETE ; int_T sigDimsArray [
1 ] = { 6 } ; sigDims . nDims = 1 ; sigDims . dimensions = sigDimsArray ;
srcInfo . numBlockPathElems = 1 ; srcInfo . fullBlockPath = ( sdiFullBlkPathU
) & blockPath ; srcInfo . SID = ( sdiSignalIDU ) & blockSID ; srcInfo .
subPath = subPath ; srcInfo . portIndex = 0 + 1 ; srcInfo . signalName =
sigName ; srcInfo . sigSourceUUID = 0 ; rtDW . knk4hpmg1v . AQHandles =
sdiStartAsyncioQueueCreation ( hDT , & srcInfo , rt_dataMapInfo . mmi .
InstanceMap . fullPath , "2d832c61-b89b-4159-9670-b79a2f0214c2" ,
sigComplexity , & sigDims , DIMENSIONS_MODE_FIXED , stCont , "1" ) ;
sdiCompleteAsyncioQueueCreation ( rtDW . knk4hpmg1v . AQHandles , hDT , &
srcInfo ) ; if ( rtDW . knk4hpmg1v . AQHandles ) {
sdiSetSignalSampleTimeString ( rtDW . knk4hpmg1v . AQHandles , "0.005" ,
0.005 , ssGetTFinal ( rtS ) ) ; sdiSetSignalRefRate ( rtDW . knk4hpmg1v .
AQHandles , 0.0 ) ; sdiSetRunStartTime ( rtDW . knk4hpmg1v . AQHandles ,
ssGetTaskTime ( rtS , 2 ) ) ; sdiAsyncRepoSetSignalExportSettings ( rtDW .
knk4hpmg1v . AQHandles , 1 , 0 ) ; sdiAsyncRepoSetSignalExportName ( rtDW .
knk4hpmg1v . AQHandles , loggedName , origSigName , propName ) ; }
sdiFreeLabel ( sigName ) ; sdiFreeLabel ( loggedName ) ; sdiFreeLabel (
origSigName ) ; sdiFreeLabel ( propName ) ; sdiFreeLabel ( blockPath ) ;
sdiFreeLabel ( blockSID ) ; sdiFreeLabel ( subPath ) ; } } if ( !
isStreamoutAlreadyRegistered ) { } } } } { { { bool
isStreamoutAlreadyRegistered = false ; { sdiSignalSourceInfoU srcInfo ;
sdiLabelU loggedName = sdiGetLabelFromChars ( "Joint2Pos" ) ; sdiLabelU
origSigName = sdiGetLabelFromChars ( "Joint2Pos" ) ; sdiLabelU propName =
sdiGetLabelFromChars ( "" ) ; sdiLabelU blockPath = sdiGetLabelFromChars (
 "openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/PS-Simulink Converter1"
) ; sdiLabelU blockSID = sdiGetLabelFromChars ( "" ) ; sdiLabelU subPath =
sdiGetLabelFromChars ( "" ) ; sdiDims sigDims ; sdiLabelU sigName =
sdiGetLabelFromChars ( "Joint2Pos" ) ; sdiAsyncRepoDataTypeHandle hDT =
sdiAsyncRepoGetBuiltInDataTypeHandle ( DATA_TYPE_DOUBLE ) ; { sdiComplexity
sigComplexity = REAL ; sdiSampleTimeContinuity stCont =
SAMPLE_TIME_CONTINUOUS ; int_T sigDimsArray [ 1 ] = { 1 } ; sigDims . nDims =
1 ; sigDims . dimensions = sigDimsArray ; srcInfo . numBlockPathElems = 1 ;
srcInfo . fullBlockPath = ( sdiFullBlkPathU ) & blockPath ; srcInfo . SID = (
sdiSignalIDU ) & blockSID ; srcInfo . subPath = subPath ; srcInfo . portIndex
= 0 + 1 ; srcInfo . signalName = sigName ; srcInfo . sigSourceUUID = 0 ; rtDW
. hbahckh543 . AQHandles = sdiStartAsyncioQueueCreation ( hDT , & srcInfo ,
rt_dataMapInfo . mmi . InstanceMap . fullPath ,
"51d9f4bb-81b7-437d-95b2-193c28a68b5c" , sigComplexity , & sigDims ,
DIMENSIONS_MODE_FIXED , stCont , "1" ) ; sdiCompleteAsyncioQueueCreation (
rtDW . hbahckh543 . AQHandles , hDT , & srcInfo ) ; if ( rtDW . hbahckh543 .
AQHandles ) { sdiSetSignalSampleTimeString ( rtDW . hbahckh543 . AQHandles ,
"&#x8FDE;&#x7EED;" , 0.0 , ssGetTFinal ( rtS ) ) ; sdiSetSignalRefRate ( rtDW
. hbahckh543 . AQHandles , 0.0 ) ; sdiSetRunStartTime ( rtDW . hbahckh543 .
AQHandles , ssGetTaskTime ( rtS , 1 ) ) ; sdiAsyncRepoSetSignalExportSettings
( rtDW . hbahckh543 . AQHandles , 1 , 0 ) ; sdiAsyncRepoSetSignalExportName (
rtDW . hbahckh543 . AQHandles , loggedName , origSigName , propName ) ; }
sdiFreeLabel ( sigName ) ; sdiFreeLabel ( loggedName ) ; sdiFreeLabel (
origSigName ) ; sdiFreeLabel ( propName ) ; sdiFreeLabel ( blockPath ) ;
sdiFreeLabel ( blockSID ) ; sdiFreeLabel ( subPath ) ; } } if ( !
isStreamoutAlreadyRegistered ) { } } } } { { { bool
isStreamoutAlreadyRegistered = false ; { sdiSignalSourceInfoU srcInfo ;
sdiLabelU loggedName = sdiGetLabelFromChars ( "Joint3Pos" ) ; sdiLabelU
origSigName = sdiGetLabelFromChars ( "Joint3Pos" ) ; sdiLabelU propName =
sdiGetLabelFromChars ( "" ) ; sdiLabelU blockPath = sdiGetLabelFromChars (
 "openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/PS-Simulink Converter2"
) ; sdiLabelU blockSID = sdiGetLabelFromChars ( "" ) ; sdiLabelU subPath =
sdiGetLabelFromChars ( "" ) ; sdiDims sigDims ; sdiLabelU sigName =
sdiGetLabelFromChars ( "Joint3Pos" ) ; sdiAsyncRepoDataTypeHandle hDT =
sdiAsyncRepoGetBuiltInDataTypeHandle ( DATA_TYPE_DOUBLE ) ; { sdiComplexity
sigComplexity = REAL ; sdiSampleTimeContinuity stCont =
SAMPLE_TIME_CONTINUOUS ; int_T sigDimsArray [ 1 ] = { 1 } ; sigDims . nDims =
1 ; sigDims . dimensions = sigDimsArray ; srcInfo . numBlockPathElems = 1 ;
srcInfo . fullBlockPath = ( sdiFullBlkPathU ) & blockPath ; srcInfo . SID = (
sdiSignalIDU ) & blockSID ; srcInfo . subPath = subPath ; srcInfo . portIndex
= 0 + 1 ; srcInfo . signalName = sigName ; srcInfo . sigSourceUUID = 0 ; rtDW
. oesft45bii . AQHandles = sdiStartAsyncioQueueCreation ( hDT , & srcInfo ,
rt_dataMapInfo . mmi . InstanceMap . fullPath ,
"59b7de4c-c408-4719-98e7-177814515bd1" , sigComplexity , & sigDims ,
DIMENSIONS_MODE_FIXED , stCont , "1" ) ; sdiCompleteAsyncioQueueCreation (
rtDW . oesft45bii . AQHandles , hDT , & srcInfo ) ; if ( rtDW . oesft45bii .
AQHandles ) { sdiSetSignalSampleTimeString ( rtDW . oesft45bii . AQHandles ,
"&#x8FDE;&#x7EED;" , 0.0 , ssGetTFinal ( rtS ) ) ; sdiSetSignalRefRate ( rtDW
. oesft45bii . AQHandles , 0.0 ) ; sdiSetRunStartTime ( rtDW . oesft45bii .
AQHandles , ssGetTaskTime ( rtS , 1 ) ) ; sdiAsyncRepoSetSignalExportSettings
( rtDW . oesft45bii . AQHandles , 1 , 0 ) ; sdiAsyncRepoSetSignalExportName (
rtDW . oesft45bii . AQHandles , loggedName , origSigName , propName ) ; }
sdiFreeLabel ( sigName ) ; sdiFreeLabel ( loggedName ) ; sdiFreeLabel (
origSigName ) ; sdiFreeLabel ( propName ) ; sdiFreeLabel ( blockPath ) ;
sdiFreeLabel ( blockSID ) ; sdiFreeLabel ( subPath ) ; } } if ( !
isStreamoutAlreadyRegistered ) { } } } } { { { bool
isStreamoutAlreadyRegistered = false ; { sdiSignalSourceInfoU srcInfo ;
sdiLabelU loggedName = sdiGetLabelFromChars ( "Joint4Pos" ) ; sdiLabelU
origSigName = sdiGetLabelFromChars ( "Joint4Pos" ) ; sdiLabelU propName =
sdiGetLabelFromChars ( "" ) ; sdiLabelU blockPath = sdiGetLabelFromChars (
 "openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/PS-Simulink Converter3"
) ; sdiLabelU blockSID = sdiGetLabelFromChars ( "" ) ; sdiLabelU subPath =
sdiGetLabelFromChars ( "" ) ; sdiDims sigDims ; sdiLabelU sigName =
sdiGetLabelFromChars ( "Joint4Pos" ) ; sdiAsyncRepoDataTypeHandle hDT =
sdiAsyncRepoGetBuiltInDataTypeHandle ( DATA_TYPE_DOUBLE ) ; { sdiComplexity
sigComplexity = REAL ; sdiSampleTimeContinuity stCont =
SAMPLE_TIME_CONTINUOUS ; int_T sigDimsArray [ 1 ] = { 1 } ; sigDims . nDims =
1 ; sigDims . dimensions = sigDimsArray ; srcInfo . numBlockPathElems = 1 ;
srcInfo . fullBlockPath = ( sdiFullBlkPathU ) & blockPath ; srcInfo . SID = (
sdiSignalIDU ) & blockSID ; srcInfo . subPath = subPath ; srcInfo . portIndex
= 0 + 1 ; srcInfo . signalName = sigName ; srcInfo . sigSourceUUID = 0 ; rtDW
. nzulbnsjry . AQHandles = sdiStartAsyncioQueueCreation ( hDT , & srcInfo ,
rt_dataMapInfo . mmi . InstanceMap . fullPath ,
"2de9fb4b-c207-4816-964c-777cd6e3d356" , sigComplexity , & sigDims ,
DIMENSIONS_MODE_FIXED , stCont , "1" ) ; sdiCompleteAsyncioQueueCreation (
rtDW . nzulbnsjry . AQHandles , hDT , & srcInfo ) ; if ( rtDW . nzulbnsjry .
AQHandles ) { sdiSetSignalSampleTimeString ( rtDW . nzulbnsjry . AQHandles ,
"&#x8FDE;&#x7EED;" , 0.0 , ssGetTFinal ( rtS ) ) ; sdiSetSignalRefRate ( rtDW
. nzulbnsjry . AQHandles , 0.0 ) ; sdiSetRunStartTime ( rtDW . nzulbnsjry .
AQHandles , ssGetTaskTime ( rtS , 1 ) ) ; sdiAsyncRepoSetSignalExportSettings
( rtDW . nzulbnsjry . AQHandles , 1 , 0 ) ; sdiAsyncRepoSetSignalExportName (
rtDW . nzulbnsjry . AQHandles , loggedName , origSigName , propName ) ; }
sdiFreeLabel ( sigName ) ; sdiFreeLabel ( loggedName ) ; sdiFreeLabel (
origSigName ) ; sdiFreeLabel ( propName ) ; sdiFreeLabel ( blockPath ) ;
sdiFreeLabel ( blockSID ) ; sdiFreeLabel ( subPath ) ; } } if ( !
isStreamoutAlreadyRegistered ) { } } } } { { { bool
isStreamoutAlreadyRegistered = false ; { sdiSignalSourceInfoU srcInfo ;
sdiLabelU loggedName = sdiGetLabelFromChars ( "GripSubPos" ) ; sdiLabelU
origSigName = sdiGetLabelFromChars ( "GripSubPos" ) ; sdiLabelU propName =
sdiGetLabelFromChars ( "" ) ; sdiLabelU blockPath = sdiGetLabelFromChars (
 "openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/PS-Simulink Converter4"
) ; sdiLabelU blockSID = sdiGetLabelFromChars ( "" ) ; sdiLabelU subPath =
sdiGetLabelFromChars ( "" ) ; sdiDims sigDims ; sdiLabelU sigName =
sdiGetLabelFromChars ( "GripSubPos" ) ; sdiAsyncRepoDataTypeHandle hDT =
sdiAsyncRepoGetBuiltInDataTypeHandle ( DATA_TYPE_DOUBLE ) ; { sdiComplexity
sigComplexity = REAL ; sdiSampleTimeContinuity stCont =
SAMPLE_TIME_CONTINUOUS ; int_T sigDimsArray [ 1 ] = { 1 } ; sigDims . nDims =
1 ; sigDims . dimensions = sigDimsArray ; srcInfo . numBlockPathElems = 1 ;
srcInfo . fullBlockPath = ( sdiFullBlkPathU ) & blockPath ; srcInfo . SID = (
sdiSignalIDU ) & blockSID ; srcInfo . subPath = subPath ; srcInfo . portIndex
= 0 + 1 ; srcInfo . signalName = sigName ; srcInfo . sigSourceUUID = 0 ; rtDW
. ppxa1k2ehz . AQHandles = sdiStartAsyncioQueueCreation ( hDT , & srcInfo ,
rt_dataMapInfo . mmi . InstanceMap . fullPath ,
"344f4cfa-2409-478c-887f-018354d995b7" , sigComplexity , & sigDims ,
DIMENSIONS_MODE_FIXED , stCont , "1" ) ; sdiCompleteAsyncioQueueCreation (
rtDW . ppxa1k2ehz . AQHandles , hDT , & srcInfo ) ; if ( rtDW . ppxa1k2ehz .
AQHandles ) { sdiSetSignalSampleTimeString ( rtDW . ppxa1k2ehz . AQHandles ,
"&#x8FDE;&#x7EED;" , 0.0 , ssGetTFinal ( rtS ) ) ; sdiSetSignalRefRate ( rtDW
. ppxa1k2ehz . AQHandles , 0.0 ) ; sdiSetRunStartTime ( rtDW . ppxa1k2ehz .
AQHandles , ssGetTaskTime ( rtS , 1 ) ) ; sdiAsyncRepoSetSignalExportSettings
( rtDW . ppxa1k2ehz . AQHandles , 1 , 0 ) ; sdiAsyncRepoSetSignalExportName (
rtDW . ppxa1k2ehz . AQHandles , loggedName , origSigName , propName ) ; }
sdiFreeLabel ( sigName ) ; sdiFreeLabel ( loggedName ) ; sdiFreeLabel (
origSigName ) ; sdiFreeLabel ( propName ) ; sdiFreeLabel ( blockPath ) ;
sdiFreeLabel ( blockSID ) ; sdiFreeLabel ( subPath ) ; } } if ( !
isStreamoutAlreadyRegistered ) { } } } } { { { bool
isStreamoutAlreadyRegistered = false ; { sdiSignalSourceInfoU srcInfo ;
sdiLabelU loggedName = sdiGetLabelFromChars ( "GripPos" ) ; sdiLabelU
origSigName = sdiGetLabelFromChars ( "GripPos" ) ; sdiLabelU propName =
sdiGetLabelFromChars ( "" ) ; sdiLabelU blockPath = sdiGetLabelFromChars (
 "openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/PS-Simulink Converter5"
) ; sdiLabelU blockSID = sdiGetLabelFromChars ( "" ) ; sdiLabelU subPath =
sdiGetLabelFromChars ( "" ) ; sdiDims sigDims ; sdiLabelU sigName =
sdiGetLabelFromChars ( "GripPos" ) ; sdiAsyncRepoDataTypeHandle hDT =
sdiAsyncRepoGetBuiltInDataTypeHandle ( DATA_TYPE_DOUBLE ) ; { sdiComplexity
sigComplexity = REAL ; sdiSampleTimeContinuity stCont =
SAMPLE_TIME_CONTINUOUS ; int_T sigDimsArray [ 1 ] = { 1 } ; sigDims . nDims =
1 ; sigDims . dimensions = sigDimsArray ; srcInfo . numBlockPathElems = 1 ;
srcInfo . fullBlockPath = ( sdiFullBlkPathU ) & blockPath ; srcInfo . SID = (
sdiSignalIDU ) & blockSID ; srcInfo . subPath = subPath ; srcInfo . portIndex
= 0 + 1 ; srcInfo . signalName = sigName ; srcInfo . sigSourceUUID = 0 ; rtDW
. axrm20twhx . AQHandles = sdiStartAsyncioQueueCreation ( hDT , & srcInfo ,
rt_dataMapInfo . mmi . InstanceMap . fullPath ,
"de250a33-99f3-4d57-8dd0-c48f8bd79833" , sigComplexity , & sigDims ,
DIMENSIONS_MODE_FIXED , stCont , "1" ) ; sdiCompleteAsyncioQueueCreation (
rtDW . axrm20twhx . AQHandles , hDT , & srcInfo ) ; if ( rtDW . axrm20twhx .
AQHandles ) { sdiSetSignalSampleTimeString ( rtDW . axrm20twhx . AQHandles ,
"&#x8FDE;&#x7EED;" , 0.0 , ssGetTFinal ( rtS ) ) ; sdiSetSignalRefRate ( rtDW
. axrm20twhx . AQHandles , 0.0 ) ; sdiSetRunStartTime ( rtDW . axrm20twhx .
AQHandles , ssGetTaskTime ( rtS , 1 ) ) ; sdiAsyncRepoSetSignalExportSettings
( rtDW . axrm20twhx . AQHandles , 1 , 0 ) ; sdiAsyncRepoSetSignalExportName (
rtDW . axrm20twhx . AQHandles , loggedName , origSigName , propName ) ; }
sdiFreeLabel ( sigName ) ; sdiFreeLabel ( loggedName ) ; sdiFreeLabel (
origSigName ) ; sdiFreeLabel ( propName ) ; sdiFreeLabel ( blockPath ) ;
sdiFreeLabel ( blockSID ) ; sdiFreeLabel ( subPath ) ; } } if ( !
isStreamoutAlreadyRegistered ) { } } } } { { { bool
isStreamoutAlreadyRegistered = false ; { sdiSignalSourceInfoU srcInfo ;
sdiLabelU loggedName = sdiGetLabelFromChars ( "Joint1Pos" ) ; sdiLabelU
origSigName = sdiGetLabelFromChars ( "Joint1Pos" ) ; sdiLabelU propName =
sdiGetLabelFromChars ( "" ) ; sdiLabelU blockPath = sdiGetLabelFromChars (
 "openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/PS-Simulink Converter"
) ; sdiLabelU blockSID = sdiGetLabelFromChars ( "" ) ; sdiLabelU subPath =
sdiGetLabelFromChars ( "" ) ; sdiDims sigDims ; sdiLabelU sigName =
sdiGetLabelFromChars ( "Joint1Pos" ) ; sdiAsyncRepoDataTypeHandle hDT =
sdiAsyncRepoGetBuiltInDataTypeHandle ( DATA_TYPE_DOUBLE ) ; { sdiComplexity
sigComplexity = REAL ; sdiSampleTimeContinuity stCont =
SAMPLE_TIME_CONTINUOUS ; int_T sigDimsArray [ 1 ] = { 1 } ; sigDims . nDims =
1 ; sigDims . dimensions = sigDimsArray ; srcInfo . numBlockPathElems = 1 ;
srcInfo . fullBlockPath = ( sdiFullBlkPathU ) & blockPath ; srcInfo . SID = (
sdiSignalIDU ) & blockSID ; srcInfo . subPath = subPath ; srcInfo . portIndex
= 0 + 1 ; srcInfo . signalName = sigName ; srcInfo . sigSourceUUID = 0 ; rtDW
. dygnyfsjhq . AQHandles = sdiStartAsyncioQueueCreation ( hDT , & srcInfo ,
rt_dataMapInfo . mmi . InstanceMap . fullPath ,
"d2439e98-8495-4456-91b6-ec2caa8eeae4" , sigComplexity , & sigDims ,
DIMENSIONS_MODE_FIXED , stCont , "1" ) ; sdiCompleteAsyncioQueueCreation (
rtDW . dygnyfsjhq . AQHandles , hDT , & srcInfo ) ; if ( rtDW . dygnyfsjhq .
AQHandles ) { sdiSetSignalSampleTimeString ( rtDW . dygnyfsjhq . AQHandles ,
"&#x8FDE;&#x7EED;" , 0.0 , ssGetTFinal ( rtS ) ) ; sdiSetSignalRefRate ( rtDW
. dygnyfsjhq . AQHandles , 0.0 ) ; sdiSetRunStartTime ( rtDW . dygnyfsjhq .
AQHandles , ssGetTaskTime ( rtS , 1 ) ) ; sdiAsyncRepoSetSignalExportSettings
( rtDW . dygnyfsjhq . AQHandles , 1 , 0 ) ; sdiAsyncRepoSetSignalExportName (
rtDW . dygnyfsjhq . AQHandles , loggedName , origSigName , propName ) ; }
sdiFreeLabel ( sigName ) ; sdiFreeLabel ( loggedName ) ; sdiFreeLabel (
origSigName ) ; sdiFreeLabel ( propName ) ; sdiFreeLabel ( blockPath ) ;
sdiFreeLabel ( blockSID ) ; sdiFreeLabel ( subPath ) ; } } if ( !
isStreamoutAlreadyRegistered ) { } } } } { FWksInfo * fromwksInfo ; if ( (
fromwksInfo = ( FWksInfo * ) calloc ( 1 , sizeof ( FWksInfo ) ) ) == ( NULL )
) { ssSetErrorStatus ( rtS ,
"from workspace STRING(Name) memory allocation error" ) ; } else {
fromwksInfo -> origWorkspaceVarName = "tuvar" ; fromwksInfo -> origDataTypeId
= 0 ; fromwksInfo -> origIsComplex = 0 ; fromwksInfo -> origWidth = 1 ;
fromwksInfo -> origElSize = sizeof ( real_T ) ; fromwksInfo -> data = ( void
* ) rtP . FromWs_Data0 ; fromwksInfo -> nDataPoints = 2 ; fromwksInfo -> time
= ( double * ) rtP . FromWs_Time0 ; rtDW . om1scnv1yi . TimePtr = fromwksInfo
-> time ; rtDW . om1scnv1yi . DataPtr = fromwksInfo -> data ; rtDW .
om1scnv1yi . RSimInfoPtr = fromwksInfo ; } rtDW . l1cob02vt3 . PrevIndex = 0
; } rtDW . pxayrmk0dv = true ; rtDW . f0qx01hjkp . isInitialized = 1 ; for (
i = 0 ; i < 6 ; i ++ ) { rtB . ojtckfkjyo [ i ] = rtP .
RateTransition6_InitialCondition ; } ecqu5ikgmi ( & rtDW . ggw1b1lkek ) ; for
( i = 0 ; i < 10 ; i ++ ) { rtDW . ggw1b1lkek . IKInternal . _pobj4 . _pobj1
[ i ] . matlabCodegenIsDeleted = true ; } for ( i = 0 ; i < 19 ; i ++ ) {
rtDW . ggw1b1lkek . IKInternal . _pobj3 [ i ] . matlabCodegenIsDeleted = true
; } for ( i = 0 ; i < 19 ; i ++ ) { rtDW . ggw1b1lkek . TreeInternal . _pobj0
[ i ] . matlabCodegenIsDeleted = true ; } for ( i = 0 ; i < 9 ; i ++ ) { rtDW
. ggw1b1lkek . IKInternal . _pobj4 . _pobj0 [ i ] . matlabCodegenIsDeleted =
true ; } rtDW . ggw1b1lkek . IKInternal . _pobj4 . Base .
matlabCodegenIsDeleted = true ; for ( i = 0 ; i < 9 ; i ++ ) { rtDW .
ggw1b1lkek . IKInternal . _pobj2 [ i ] . matlabCodegenIsDeleted = true ; }
for ( i = 0 ; i < 18 ; i ++ ) { rtDW . ggw1b1lkek . TreeInternal . _pobj2 [ i
] . matlabCodegenIsDeleted = true ; } rtDW . ggw1b1lkek . TreeInternal . Base
. matlabCodegenIsDeleted = true ; rtDW . ggw1b1lkek . TreeInternal .
matlabCodegenIsDeleted = true ; rtDW . ggw1b1lkek . IKInternal . _pobj4 .
matlabCodegenIsDeleted = true ; rtDW . ggw1b1lkek . IKInternal . _pobj0 .
matlabCodegenIsDeleted = true ; rtDW . ggw1b1lkek . IKInternal . _pobj5 .
matlabCodegenIsDeleted = true ; rtDW . ggw1b1lkek . IKInternal .
matlabCodegenIsDeleted = true ; rtDW . hl2ip4fyfs = 7U ; rtDW . jvss2h1o3w =
true ; rtDW . iujlyug3z5 = 1144108930U ; rtDW . ohnifdbmla = true ; rtDW .
bhbuxnbarg [ 0 ] = 362436069U ; rtDW . bhbuxnbarg [ 1 ] = 521288629U ; rtDW .
n0rsfsdi3c = true ; memcpy ( & rtDW . ehmyrdjr0v [ 0 ] , & tmp_k [ 0 ] , 625U
* sizeof ( uint32_T ) ) ; rtDW . jmldnbipjd = true ; rtDW . m2usyxvuaa = 0U ;
rtDW . ejmahq1kwn = true ; rtDW . kad3ljw5be [ 0 ] = 362436069U ; rtDW .
kad3ljw5be [ 1 ] = 521288629U ; rtDW . kgbrg3ivnd = true ; rtDW . ggw1b1lkek
. isInitialized = 0 ; rtDW . ggw1b1lkek . matlabCodegenIsDeleted = false ;
rtDW . awistkf5gl = true ; ep2455cw5ms ( & rtDW . ggw1b1lkek ) ; ecqu5ikgmie
( & rtDW . c2ho3h3oyv ) ; for ( i = 0 ; i < 18 ; i ++ ) { rtDW . c2ho3h3oyv .
TreeInternal . _pobj0 [ i ] . CollisionsInternal . matlabCodegenIsDeleted =
true ; } rtDW . c2ho3h3oyv . TreeInternal . Base . CollisionsInternal .
matlabCodegenIsDeleted = true ; for ( i = 0 ; i < 18 ; i ++ ) { rtDW .
c2ho3h3oyv . TreeInternal . _pobj0 [ i ] . matlabCodegenIsDeleted = true ; }
rtDW . c2ho3h3oyv . TreeInternal . Base . matlabCodegenIsDeleted = true ;
rtDW . c2ho3h3oyv . TreeInternal . matlabCodegenIsDeleted = true ; rtDW .
c2ho3h3oyv . isInitialized = 0 ; rtDW . c2ho3h3oyv . matlabCodegenIsDeleted =
false ; rtDW . l11orfjj1u = true ; ep2455cw5m ( & rtDW . c2ho3h3oyv ) ; rtDW
. nervxud5ud = true ; rtDW . jffwzd0nlb . isInitialized = 1 ; tmp =
nesl_lease_simulator (
"openManipulatorTrajectoryPlanning/Solver Configuration_1" , 0 , 0 ) ; rtDW .
etcerwyaoj = ( void * ) tmp ; tmp_p = pointer_is_null ( rtDW . etcerwyaoj ) ;
if ( tmp_p ) { openManipulatorTrajectoryPlanning_6a081443_1_gateway ( ) ; tmp
= nesl_lease_simulator (
"openManipulatorTrajectoryPlanning/Solver Configuration_1" , 0 , 0 ) ; rtDW .
etcerwyaoj = ( void * ) tmp ; } slsaSaveRawMemoryForSimTargetOP ( rtS ,
"openManipulatorTrajectoryPlanning/Solver Configuration_100" , ( void * * ) (
& rtDW . etcerwyaoj ) , 0U * sizeof ( real_T ) , nesl_save_simdata ,
nesl_restore_simdata ) ; simulationData = nesl_create_simulation_data ( ) ;
rtDW . a5qgeff343 = ( void * ) simulationData ; diagnosticManager =
rtw_create_diagnostics ( ) ; rtDW . caiz5lqz5f = ( void * ) diagnosticManager
; modelParameters . mSolverType = NE_SOLVER_TYPE_DAE ; modelParameters .
mSolverAbsTol = 0.001 ; modelParameters . mSolverRelTol = 0.001 ;
modelParameters . mSolverModifyAbsTol = NE_MODIFY_ABS_TOL_MAYBE ;
modelParameters . mStartTime = 0.0 ; modelParameters . mLoadInitialState =
false ; modelParameters . mUseSimState = false ; modelParameters .
mLinTrimCompile = false ; modelParameters . mLoggingMode = SSC_LOGGING_NONE ;
modelParameters . mRTWModifiedTimeStamp = 5.91976676E+8 ; modelParameters .
mZcDisabled = false ; tmp_e = 0.001 ; modelParameters . mSolverTolerance =
tmp_e ; tmp_e = 0.0 ; modelParameters . mFixedStepSize = tmp_e ; tmp_p = true
; modelParameters . mVariableStepSolver = tmp_p ; tmp_p = false ;
modelParameters . mIsUsingODEN = tmp_p ; tmp_p =
slIsRapidAcceleratorSimulating ( ) ; val = ssGetGlobalInitialStatesAvailable
( rtS ) ; if ( tmp_p ) { val = ( val && ssIsFirstInitCond ( rtS ) ) ; }
modelParameters . mLoadInitialState = val ; modelParameters . mZcDisabled =
false ; diagnosticManager = ( NeuDiagnosticManager * ) rtDW . caiz5lqz5f ;
diagnosticTree = neu_diagnostic_manager_get_initial_tree ( diagnosticManager
) ; i = nesl_initialize_simulator ( ( NeslSimulator * ) rtDW . etcerwyaoj , &
modelParameters , diagnosticManager ) ; if ( i != 0 ) { tmp_p =
error_buffer_is_empty ( ssGetErrorStatus ( rtS ) ) ; if ( tmp_p ) { msg =
rtw_diagnostics_msg ( diagnosticTree ) ; ssSetErrorStatus ( rtS , msg ) ; } }
simulationData = ( NeslSimulationData * ) rtDW . a5qgeff343 ; time = ssGetT (
rtS ) ; simulationData -> mData -> mTime . mN = 1 ; simulationData -> mData
-> mTime . mX = & time ; simulationData -> mData -> mContStates . mN = 12 ;
simulationData -> mData -> mContStates . mX = & rtX . ge5rtzzo4y [ 0 ] ;
simulationData -> mData -> mDiscStates . mN = 0 ; simulationData -> mData ->
mDiscStates . mX = & rtDW . ofd1yxgqn2 ; simulationData -> mData ->
mModeVector . mN = 0 ; simulationData -> mData -> mModeVector . mX = & rtDW .
edj0t2oelw ; tmp_p = ( ssIsMajorTimeStep ( rtS ) && ssGetRTWSolverInfo ( rtS
) -> foundContZcEvents ) ; simulationData -> mData -> mFoundZcEvents = tmp_p
; simulationData -> mData -> mIsMajorTimeStep = ssIsMajorTimeStep ( rtS ) ;
tmp_p = ( ssGetMdlInfoPtr ( rtS ) -> mdlFlags . solverAssertCheck == 1U ) ;
simulationData -> mData -> mIsSolverAssertCheck = tmp_p ; tmp_p =
ssIsSolverCheckingCIC ( rtS ) ; simulationData -> mData ->
mIsSolverCheckingCIC = tmp_p ; tmp_p = ssIsSolverComputingJacobian ( rtS ) ;
simulationData -> mData -> mIsComputingJacobian = tmp_p ; simulationData ->
mData -> mIsEvaluatingF0 = ( ssGetEvaluatingF0ForJacobian ( rtS ) != 0 ) ;
tmp_p = ssIsSolverRequestingReset ( rtS ) ; simulationData -> mData ->
mIsSolverRequestingReset = tmp_p ; simulationData -> mData ->
mIsModeUpdateTimeStep = ssIsModeUpdateTimeStep ( rtS ) ; tmp_m [ 0 ] = 0 ;
tmp_i [ 0 ] = rtB . b5i1gadrfg [ 0 ] ; tmp_i [ 1 ] = rtB . b5i1gadrfg [ 1 ] ;
tmp_i [ 2 ] = rtB . b5i1gadrfg [ 2 ] ; tmp_i [ 3 ] = rtB . b5i1gadrfg [ 3 ] ;
tmp_m [ 1 ] = 4 ; tmp_i [ 4 ] = rtB . o4mrrxpgqr [ 0 ] ; tmp_i [ 5 ] = rtB .
o4mrrxpgqr [ 1 ] ; tmp_i [ 6 ] = rtB . o4mrrxpgqr [ 2 ] ; tmp_i [ 7 ] = rtB .
o4mrrxpgqr [ 3 ] ; tmp_m [ 2 ] = 8 ; tmp_i [ 8 ] = rtB . bpxoa5jqph [ 0 ] ;
tmp_i [ 9 ] = rtB . bpxoa5jqph [ 1 ] ; tmp_i [ 10 ] = rtB . bpxoa5jqph [ 2 ]
; tmp_i [ 11 ] = rtB . bpxoa5jqph [ 3 ] ; tmp_m [ 3 ] = 12 ; tmp_i [ 12 ] =
rtB . gtqltersrn [ 0 ] ; tmp_i [ 13 ] = rtB . gtqltersrn [ 1 ] ; tmp_i [ 14 ]
= rtB . gtqltersrn [ 2 ] ; tmp_i [ 15 ] = rtB . gtqltersrn [ 3 ] ; tmp_m [ 4
] = 16 ; tmp_i [ 16 ] = rtB . kfmkqerxag [ 0 ] ; tmp_i [ 17 ] = rtB .
kfmkqerxag [ 1 ] ; tmp_i [ 18 ] = rtB . kfmkqerxag [ 2 ] ; tmp_i [ 19 ] = rtB
. kfmkqerxag [ 3 ] ; tmp_m [ 5 ] = 20 ; tmp_i [ 20 ] = rtB . kqkimrfdm5 [ 0 ]
; tmp_i [ 21 ] = rtB . kqkimrfdm5 [ 1 ] ; tmp_i [ 22 ] = rtB . kqkimrfdm5 [ 2
] ; tmp_i [ 23 ] = rtB . kqkimrfdm5 [ 3 ] ; tmp_m [ 6 ] = 24 ; tmp_i [ 24 ] =
rtB . ffuvz5uef3 [ 0 ] ; tmp_i [ 25 ] = rtB . ffuvz5uef3 [ 1 ] ; tmp_i [ 26 ]
= rtB . ffuvz5uef3 [ 2 ] ; tmp_i [ 27 ] = rtB . ffuvz5uef3 [ 3 ] ; tmp_m [ 7
] = 28 ; tmp_i [ 28 ] = rtB . j5luahqacu [ 0 ] ; tmp_i [ 29 ] = rtB .
j5luahqacu [ 1 ] ; tmp_i [ 30 ] = rtB . j5luahqacu [ 2 ] ; tmp_i [ 31 ] = rtB
. j5luahqacu [ 3 ] ; tmp_m [ 8 ] = 32 ; tmp_i [ 32 ] = rtB . ecaqpvxc3x [ 0 ]
; tmp_i [ 33 ] = rtB . ecaqpvxc3x [ 1 ] ; tmp_i [ 34 ] = rtB . ecaqpvxc3x [ 2
] ; tmp_i [ 35 ] = rtB . ecaqpvxc3x [ 3 ] ; tmp_m [ 9 ] = 36 ; tmp_i [ 36 ] =
rtB . d5kmywozm5 [ 0 ] ; tmp_i [ 37 ] = rtB . d5kmywozm5 [ 1 ] ; tmp_i [ 38 ]
= rtB . d5kmywozm5 [ 2 ] ; tmp_i [ 39 ] = rtB . d5kmywozm5 [ 3 ] ; tmp_m [ 10
] = 40 ; simulationData -> mData -> mInputValues . mN = 40 ; simulationData
-> mData -> mInputValues . mX = & tmp_i [ 0 ] ; simulationData -> mData ->
mInputOffsets . mN = 11 ; simulationData -> mData -> mInputOffsets . mX = &
tmp_m [ 0 ] ; simulationData -> mData -> mNumjacDxLo . mN = 12 ;
simulationData -> mData -> mNumjacDxLo . mX = & _rtXPerturbMin -> ge5rtzzo4y
[ 0 ] ; simulationData -> mData -> mNumjacDxHi . mN = 12 ; simulationData ->
mData -> mNumjacDxHi . mX = & _rtXPerturbMax -> ge5rtzzo4y [ 0 ] ;
diagnosticManager = ( NeuDiagnosticManager * ) rtDW . caiz5lqz5f ;
diagnosticTree_p = neu_diagnostic_manager_get_initial_tree (
diagnosticManager ) ; i = ne_simulator_method ( ( NeslSimulator * ) rtDW .
etcerwyaoj , NESL_SIM_NUMJAC_DX_BOUNDS , simulationData , diagnosticManager )
; if ( i != 0 ) { tmp_p = error_buffer_is_empty ( ssGetErrorStatus ( rtS ) )
; if ( tmp_p ) { msg_p = rtw_diagnostics_msg ( diagnosticTree_p ) ;
ssSetErrorStatus ( rtS , msg_p ) ; } } tmp = nesl_lease_simulator (
"openManipulatorTrajectoryPlanning/Solver Configuration_1" , 1 , 0 ) ; rtDW .
oibswdkijz = ( void * ) tmp ; tmp_p = pointer_is_null ( rtDW . oibswdkijz ) ;
if ( tmp_p ) { openManipulatorTrajectoryPlanning_6a081443_1_gateway ( ) ; tmp
= nesl_lease_simulator (
"openManipulatorTrajectoryPlanning/Solver Configuration_1" , 1 , 0 ) ; rtDW .
oibswdkijz = ( void * ) tmp ; } slsaSaveRawMemoryForSimTargetOP ( rtS ,
"openManipulatorTrajectoryPlanning/Solver Configuration_110" , ( void * * ) (
& rtDW . oibswdkijz ) , 0U * sizeof ( real_T ) , nesl_save_simdata ,
nesl_restore_simdata ) ; simulationData = nesl_create_simulation_data ( ) ;
rtDW . papxjgo3op = ( void * ) simulationData ; diagnosticManager =
rtw_create_diagnostics ( ) ; rtDW . auudttxoyr = ( void * ) diagnosticManager
; modelParameters_p . mSolverType = NE_SOLVER_TYPE_DAE ; modelParameters_p .
mSolverAbsTol = 0.001 ; modelParameters_p . mSolverRelTol = 0.001 ;
modelParameters_p . mSolverModifyAbsTol = NE_MODIFY_ABS_TOL_MAYBE ;
modelParameters_p . mStartTime = 0.0 ; modelParameters_p . mLoadInitialState
= false ; modelParameters_p . mUseSimState = false ; modelParameters_p .
mLinTrimCompile = false ; modelParameters_p . mLoggingMode = SSC_LOGGING_NONE
; modelParameters_p . mRTWModifiedTimeStamp = 5.91976676E+8 ;
modelParameters_p . mZcDisabled = false ; tmp_e = 0.001 ; modelParameters_p .
mSolverTolerance = tmp_e ; tmp_e = 0.0 ; modelParameters_p . mFixedStepSize =
tmp_e ; tmp_p = true ; modelParameters_p . mVariableStepSolver = tmp_p ;
tmp_p = false ; modelParameters_p . mIsUsingODEN = tmp_p ; tmp_p =
slIsRapidAcceleratorSimulating ( ) ; val = ssGetGlobalInitialStatesAvailable
( rtS ) ; if ( tmp_p ) { val = ( val && ssIsFirstInitCond ( rtS ) ) ; }
modelParameters_p . mLoadInitialState = val ; modelParameters_p . mZcDisabled
= false ; diagnosticManager = ( NeuDiagnosticManager * ) rtDW . auudttxoyr ;
diagnosticTree_e = neu_diagnostic_manager_get_initial_tree (
diagnosticManager ) ; i = nesl_initialize_simulator ( ( NeslSimulator * )
rtDW . oibswdkijz , & modelParameters_p , diagnosticManager ) ; if ( i != 0 )
{ tmp_p = error_buffer_is_empty ( ssGetErrorStatus ( rtS ) ) ; if ( tmp_p ) {
msg_e = rtw_diagnostics_msg ( diagnosticTree_e ) ; ssSetErrorStatus ( rtS ,
msg_e ) ; } } ecqu5ikgmiel ( & rtDW . pqz301f1cc ) ; for ( i = 0 ; i < 18 ; i
++ ) { rtDW . pqz301f1cc . TreeInternal . _pobj0 [ i ] . CollisionsInternal .
matlabCodegenIsDeleted = true ; } rtDW . pqz301f1cc . TreeInternal . Base .
CollisionsInternal . matlabCodegenIsDeleted = true ; for ( i = 0 ; i < 18 ; i
++ ) { rtDW . pqz301f1cc . TreeInternal . _pobj0 [ i ] .
matlabCodegenIsDeleted = true ; } rtDW . pqz301f1cc . TreeInternal . Base .
matlabCodegenIsDeleted = true ; rtDW . pqz301f1cc . TreeInternal .
matlabCodegenIsDeleted = true ; rtDW . pqz301f1cc . isInitialized = 0 ; rtDW
. pqz301f1cc . matlabCodegenIsDeleted = false ; rtDW . kta541utr1 = true ;
ep2455cw5msd ( & rtDW . pqz301f1cc ) ; for ( i = 0 ; i < 6 ; i ++ ) { rtB .
nd1rapbf2u [ i ] = rtP . RateTransition_InitialCondition ; } tmp =
nesl_lease_simulator (
"openManipulatorTrajectoryPlanning/Solver Configuration_2" , 0 , 0 ) ; rtDW .
dc0bllwdyl = ( void * ) tmp ; tmp_p = pointer_is_null ( rtDW . dc0bllwdyl ) ;
if ( tmp_p ) { openManipulatorTrajectoryPlanning_6a081443_2_gateway ( ) ; tmp
= nesl_lease_simulator (
"openManipulatorTrajectoryPlanning/Solver Configuration_2" , 0 , 0 ) ; rtDW .
dc0bllwdyl = ( void * ) tmp ; } slsaSaveRawMemoryForSimTargetOP ( rtS ,
"openManipulatorTrajectoryPlanning/Solver Configuration_200" , ( void * * ) (
& rtDW . dc0bllwdyl ) , 0U * sizeof ( real_T ) , nesl_save_simdata ,
nesl_restore_simdata ) ; simulationData = nesl_create_simulation_data ( ) ;
rtDW . khhoybnt1p = ( void * ) simulationData ; diagnosticManager =
rtw_create_diagnostics ( ) ; rtDW . gzao4gihsp = ( void * ) diagnosticManager
; modelParameters_e . mSolverType = NE_SOLVER_TYPE_DAE ; modelParameters_e .
mSolverAbsTol = 0.001 ; modelParameters_e . mSolverRelTol = 0.001 ;
modelParameters_e . mSolverModifyAbsTol = NE_MODIFY_ABS_TOL_MAYBE ;
modelParameters_e . mStartTime = 0.0 ; modelParameters_e . mLoadInitialState
= false ; modelParameters_e . mUseSimState = false ; modelParameters_e .
mLinTrimCompile = false ; modelParameters_e . mLoggingMode = SSC_LOGGING_NONE
; modelParameters_e . mRTWModifiedTimeStamp = 5.91976676E+8 ;
modelParameters_e . mZcDisabled = false ; tmp_e = 0.001 ; modelParameters_e .
mSolverTolerance = tmp_e ; tmp_e = 0.0 ; modelParameters_e . mFixedStepSize =
tmp_e ; tmp_p = true ; modelParameters_e . mVariableStepSolver = tmp_p ;
tmp_p = false ; modelParameters_e . mIsUsingODEN = tmp_p ; tmp_p =
slIsRapidAcceleratorSimulating ( ) ; val = ssGetGlobalInitialStatesAvailable
( rtS ) ; if ( tmp_p ) { val = ( val && ssIsFirstInitCond ( rtS ) ) ; }
modelParameters_e . mLoadInitialState = val ; modelParameters_e . mZcDisabled
= false ; diagnosticManager = ( NeuDiagnosticManager * ) rtDW . gzao4gihsp ;
diagnosticTree_i = neu_diagnostic_manager_get_initial_tree (
diagnosticManager ) ; i = nesl_initialize_simulator ( ( NeslSimulator * )
rtDW . dc0bllwdyl , & modelParameters_e , diagnosticManager ) ; if ( i != 0 )
{ tmp_p = error_buffer_is_empty ( ssGetErrorStatus ( rtS ) ) ; if ( tmp_p ) {
msg_i = rtw_diagnostics_msg ( diagnosticTree_i ) ; ssSetErrorStatus ( rtS ,
msg_i ) ; } } simulationData = ( NeslSimulationData * ) rtDW . khhoybnt1p ;
time_p = ssGetT ( rtS ) ; simulationData -> mData -> mTime . mN = 1 ;
simulationData -> mData -> mTime . mX = & time_p ; simulationData -> mData ->
mContStates . mN = 2 ; simulationData -> mData -> mContStates . mX = & rtX .
lnruxliw5a [ 0 ] ; simulationData -> mData -> mDiscStates . mN = 0 ;
simulationData -> mData -> mDiscStates . mX = & rtDW . awzqwdk31a ;
simulationData -> mData -> mModeVector . mN = 0 ; simulationData -> mData ->
mModeVector . mX = & rtDW . onnfopq04a ; tmp_p = ( ssIsMajorTimeStep ( rtS )
&& ssGetRTWSolverInfo ( rtS ) -> foundContZcEvents ) ; simulationData ->
mData -> mFoundZcEvents = tmp_p ; simulationData -> mData -> mIsMajorTimeStep
= ssIsMajorTimeStep ( rtS ) ; tmp_p = ( ssGetMdlInfoPtr ( rtS ) -> mdlFlags .
solverAssertCheck == 1U ) ; simulationData -> mData -> mIsSolverAssertCheck =
tmp_p ; tmp_p = ssIsSolverCheckingCIC ( rtS ) ; simulationData -> mData ->
mIsSolverCheckingCIC = tmp_p ; tmp_p = ssIsSolverComputingJacobian ( rtS ) ;
simulationData -> mData -> mIsComputingJacobian = tmp_p ; simulationData ->
mData -> mIsEvaluatingF0 = ( ssGetEvaluatingF0ForJacobian ( rtS ) != 0 ) ;
tmp_p = ssIsSolverRequestingReset ( rtS ) ; simulationData -> mData ->
mIsSolverRequestingReset = tmp_p ; simulationData -> mData ->
mIsModeUpdateTimeStep = ssIsModeUpdateTimeStep ( rtS ) ; tmp_j [ 0 ] = 0 ;
tmp_g [ 0 ] = rtB . grytuooxbk [ 0 ] ; tmp_g [ 1 ] = rtB . grytuooxbk [ 1 ] ;
tmp_g [ 2 ] = rtB . grytuooxbk [ 2 ] ; tmp_g [ 3 ] = rtB . grytuooxbk [ 3 ] ;
tmp_j [ 1 ] = 4 ; simulationData -> mData -> mInputValues . mN = 4 ;
simulationData -> mData -> mInputValues . mX = & tmp_g [ 0 ] ; simulationData
-> mData -> mInputOffsets . mN = 2 ; simulationData -> mData -> mInputOffsets
. mX = & tmp_j [ 0 ] ; simulationData -> mData -> mNumjacDxLo . mN = 2 ;
simulationData -> mData -> mNumjacDxLo . mX = & _rtXPerturbMin -> lnruxliw5a
[ 0 ] ; simulationData -> mData -> mNumjacDxHi . mN = 2 ; simulationData ->
mData -> mNumjacDxHi . mX = & _rtXPerturbMax -> lnruxliw5a [ 0 ] ;
diagnosticManager = ( NeuDiagnosticManager * ) rtDW . gzao4gihsp ;
diagnosticTree_m = neu_diagnostic_manager_get_initial_tree (
diagnosticManager ) ; i = ne_simulator_method ( ( NeslSimulator * ) rtDW .
dc0bllwdyl , NESL_SIM_NUMJAC_DX_BOUNDS , simulationData , diagnosticManager )
; if ( i != 0 ) { tmp_p = error_buffer_is_empty ( ssGetErrorStatus ( rtS ) )
; if ( tmp_p ) { msg_m = rtw_diagnostics_msg ( diagnosticTree_m ) ;
ssSetErrorStatus ( rtS , msg_m ) ; } } tmp = nesl_lease_simulator (
"openManipulatorTrajectoryPlanning/Solver Configuration_2" , 1 , 0 ) ; rtDW .
ezin4amxyy = ( void * ) tmp ; tmp_p = pointer_is_null ( rtDW . ezin4amxyy ) ;
if ( tmp_p ) { openManipulatorTrajectoryPlanning_6a081443_2_gateway ( ) ; tmp
= nesl_lease_simulator (
"openManipulatorTrajectoryPlanning/Solver Configuration_2" , 1 , 0 ) ; rtDW .
ezin4amxyy = ( void * ) tmp ; } slsaSaveRawMemoryForSimTargetOP ( rtS ,
"openManipulatorTrajectoryPlanning/Solver Configuration_210" , ( void * * ) (
& rtDW . ezin4amxyy ) , 0U * sizeof ( real_T ) , nesl_save_simdata ,
nesl_restore_simdata ) ; simulationData = nesl_create_simulation_data ( ) ;
rtDW . m2vrnf5y3k = ( void * ) simulationData ; diagnosticManager =
rtw_create_diagnostics ( ) ; rtDW . mawdbjpiix = ( void * ) diagnosticManager
; modelParameters_i . mSolverType = NE_SOLVER_TYPE_DAE ; modelParameters_i .
mSolverAbsTol = 0.001 ; modelParameters_i . mSolverRelTol = 0.001 ;
modelParameters_i . mSolverModifyAbsTol = NE_MODIFY_ABS_TOL_MAYBE ;
modelParameters_i . mStartTime = 0.0 ; modelParameters_i . mLoadInitialState
= false ; modelParameters_i . mUseSimState = false ; modelParameters_i .
mLinTrimCompile = false ; modelParameters_i . mLoggingMode = SSC_LOGGING_NONE
; modelParameters_i . mRTWModifiedTimeStamp = 5.91976676E+8 ;
modelParameters_i . mZcDisabled = false ; tmp_e = 0.001 ; modelParameters_i .
mSolverTolerance = tmp_e ; tmp_e = 0.0 ; modelParameters_i . mFixedStepSize =
tmp_e ; tmp_p = true ; modelParameters_i . mVariableStepSolver = tmp_p ;
tmp_p = false ; modelParameters_i . mIsUsingODEN = tmp_p ; tmp_p =
slIsRapidAcceleratorSimulating ( ) ; val = ssGetGlobalInitialStatesAvailable
( rtS ) ; if ( tmp_p ) { val = ( val && ssIsFirstInitCond ( rtS ) ) ; }
modelParameters_i . mLoadInitialState = val ; modelParameters_i . mZcDisabled
= false ; diagnosticManager = ( NeuDiagnosticManager * ) rtDW . mawdbjpiix ;
diagnosticTree_g = neu_diagnostic_manager_get_initial_tree (
diagnosticManager ) ; i = nesl_initialize_simulator ( ( NeslSimulator * )
rtDW . ezin4amxyy , & modelParameters_i , diagnosticManager ) ; if ( i != 0 )
{ tmp_p = error_buffer_is_empty ( ssGetErrorStatus ( rtS ) ) ; if ( tmp_p ) {
msg_g = rtw_diagnostics_msg ( diagnosticTree_g ) ; ssSetErrorStatus ( rtS ,
msg_g ) ; } } tmp = nesl_lease_simulator (
"openManipulatorTrajectoryPlanning/Solver Configuration_3" , 0 , 0 ) ; rtDW .
nqy5lwwfbt = ( void * ) tmp ; tmp_p = pointer_is_null ( rtDW . nqy5lwwfbt ) ;
if ( tmp_p ) { openManipulatorTrajectoryPlanning_6a081443_3_gateway ( ) ; tmp
= nesl_lease_simulator (
"openManipulatorTrajectoryPlanning/Solver Configuration_3" , 0 , 0 ) ; rtDW .
nqy5lwwfbt = ( void * ) tmp ; } slsaSaveRawMemoryForSimTargetOP ( rtS ,
"openManipulatorTrajectoryPlanning/Solver Configuration_300" , ( void * * ) (
& rtDW . nqy5lwwfbt ) , 0U * sizeof ( real_T ) , nesl_save_simdata ,
nesl_restore_simdata ) ; simulationData = nesl_create_simulation_data ( ) ;
rtDW . fg4qcqmocx = ( void * ) simulationData ; diagnosticManager =
rtw_create_diagnostics ( ) ; rtDW . fzld5uaeew = ( void * ) diagnosticManager
; modelParameters_m . mSolverType = NE_SOLVER_TYPE_DAE ; modelParameters_m .
mSolverAbsTol = 0.001 ; modelParameters_m . mSolverRelTol = 0.001 ;
modelParameters_m . mSolverModifyAbsTol = NE_MODIFY_ABS_TOL_MAYBE ;
modelParameters_m . mStartTime = 0.0 ; modelParameters_m . mLoadInitialState
= false ; modelParameters_m . mUseSimState = false ; modelParameters_m .
mLinTrimCompile = false ; modelParameters_m . mLoggingMode = SSC_LOGGING_NONE
; modelParameters_m . mRTWModifiedTimeStamp = 5.91976676E+8 ;
modelParameters_m . mZcDisabled = false ; tmp_e = 0.001 ; modelParameters_m .
mSolverTolerance = tmp_e ; tmp_e = 0.0 ; modelParameters_m . mFixedStepSize =
tmp_e ; tmp_p = true ; modelParameters_m . mVariableStepSolver = tmp_p ;
tmp_p = false ; modelParameters_m . mIsUsingODEN = tmp_p ; tmp_p =
slIsRapidAcceleratorSimulating ( ) ; val = ssGetGlobalInitialStatesAvailable
( rtS ) ; if ( tmp_p ) { val = ( val && ssIsFirstInitCond ( rtS ) ) ; }
modelParameters_m . mLoadInitialState = val ; modelParameters_m . mZcDisabled
= false ; diagnosticManager = ( NeuDiagnosticManager * ) rtDW . fzld5uaeew ;
diagnosticTree_j = neu_diagnostic_manager_get_initial_tree (
diagnosticManager ) ; i = nesl_initialize_simulator ( ( NeslSimulator * )
rtDW . nqy5lwwfbt , & modelParameters_m , diagnosticManager ) ; if ( i != 0 )
{ tmp_p = error_buffer_is_empty ( ssGetErrorStatus ( rtS ) ) ; if ( tmp_p ) {
msg_j = rtw_diagnostics_msg ( diagnosticTree_j ) ; ssSetErrorStatus ( rtS ,
msg_j ) ; } } simulationData = ( NeslSimulationData * ) rtDW . fg4qcqmocx ;
time_e = ssGetT ( rtS ) ; simulationData -> mData -> mTime . mN = 1 ;
simulationData -> mData -> mTime . mX = & time_e ; simulationData -> mData ->
mContStates . mN = 2 ; simulationData -> mData -> mContStates . mX = & rtX .
ipa1byflst [ 0 ] ; simulationData -> mData -> mDiscStates . mN = 0 ;
simulationData -> mData -> mDiscStates . mX = & rtDW . grpnqhsyz0 ;
simulationData -> mData -> mModeVector . mN = 0 ; simulationData -> mData ->
mModeVector . mX = & rtDW . ho0ogmrm4n ; tmp_p = ( ssIsMajorTimeStep ( rtS )
&& ssGetRTWSolverInfo ( rtS ) -> foundContZcEvents ) ; simulationData ->
mData -> mFoundZcEvents = tmp_p ; simulationData -> mData -> mIsMajorTimeStep
= ssIsMajorTimeStep ( rtS ) ; tmp_p = ( ssGetMdlInfoPtr ( rtS ) -> mdlFlags .
solverAssertCheck == 1U ) ; simulationData -> mData -> mIsSolverAssertCheck =
tmp_p ; tmp_p = ssIsSolverCheckingCIC ( rtS ) ; simulationData -> mData ->
mIsSolverCheckingCIC = tmp_p ; tmp_p = ssIsSolverComputingJacobian ( rtS ) ;
simulationData -> mData -> mIsComputingJacobian = tmp_p ; simulationData ->
mData -> mIsEvaluatingF0 = ( ssGetEvaluatingF0ForJacobian ( rtS ) != 0 ) ;
tmp_p = ssIsSolverRequestingReset ( rtS ) ; simulationData -> mData ->
mIsSolverRequestingReset = tmp_p ; simulationData -> mData ->
mIsModeUpdateTimeStep = ssIsModeUpdateTimeStep ( rtS ) ; tmp_c [ 0 ] = 0 ;
tmp_f [ 0 ] = rtB . n4hlwmps2s [ 0 ] ; tmp_f [ 1 ] = rtB . n4hlwmps2s [ 1 ] ;
tmp_f [ 2 ] = rtB . n4hlwmps2s [ 2 ] ; tmp_f [ 3 ] = rtB . n4hlwmps2s [ 3 ] ;
tmp_c [ 1 ] = 4 ; simulationData -> mData -> mInputValues . mN = 4 ;
simulationData -> mData -> mInputValues . mX = & tmp_f [ 0 ] ; simulationData
-> mData -> mInputOffsets . mN = 2 ; simulationData -> mData -> mInputOffsets
. mX = & tmp_c [ 0 ] ; simulationData -> mData -> mNumjacDxLo . mN = 2 ;
simulationData -> mData -> mNumjacDxLo . mX = & _rtXPerturbMin -> ipa1byflst
[ 0 ] ; simulationData -> mData -> mNumjacDxHi . mN = 2 ; simulationData ->
mData -> mNumjacDxHi . mX = & _rtXPerturbMax -> ipa1byflst [ 0 ] ;
diagnosticManager = ( NeuDiagnosticManager * ) rtDW . fzld5uaeew ;
diagnosticTree_f = neu_diagnostic_manager_get_initial_tree (
diagnosticManager ) ; i = ne_simulator_method ( ( NeslSimulator * ) rtDW .
nqy5lwwfbt , NESL_SIM_NUMJAC_DX_BOUNDS , simulationData , diagnosticManager )
; if ( i != 0 ) { tmp_p = error_buffer_is_empty ( ssGetErrorStatus ( rtS ) )
; if ( tmp_p ) { msg_f = rtw_diagnostics_msg ( diagnosticTree_f ) ;
ssSetErrorStatus ( rtS , msg_f ) ; } } tmp = nesl_lease_simulator (
"openManipulatorTrajectoryPlanning/Solver Configuration_3" , 1 , 0 ) ; rtDW .
fw4oj40z44 = ( void * ) tmp ; tmp_p = pointer_is_null ( rtDW . fw4oj40z44 ) ;
if ( tmp_p ) { openManipulatorTrajectoryPlanning_6a081443_3_gateway ( ) ; tmp
= nesl_lease_simulator (
"openManipulatorTrajectoryPlanning/Solver Configuration_3" , 1 , 0 ) ; rtDW .
fw4oj40z44 = ( void * ) tmp ; } slsaSaveRawMemoryForSimTargetOP ( rtS ,
"openManipulatorTrajectoryPlanning/Solver Configuration_310" , ( void * * ) (
& rtDW . fw4oj40z44 ) , 0U * sizeof ( real_T ) , nesl_save_simdata ,
nesl_restore_simdata ) ; simulationData = nesl_create_simulation_data ( ) ;
rtDW . m3o4jlnlit = ( void * ) simulationData ; diagnosticManager =
rtw_create_diagnostics ( ) ; rtDW . cdslitainn = ( void * ) diagnosticManager
; modelParameters_g . mSolverType = NE_SOLVER_TYPE_DAE ; modelParameters_g .
mSolverAbsTol = 0.001 ; modelParameters_g . mSolverRelTol = 0.001 ;
modelParameters_g . mSolverModifyAbsTol = NE_MODIFY_ABS_TOL_MAYBE ;
modelParameters_g . mStartTime = 0.0 ; modelParameters_g . mLoadInitialState
= false ; modelParameters_g . mUseSimState = false ; modelParameters_g .
mLinTrimCompile = false ; modelParameters_g . mLoggingMode = SSC_LOGGING_NONE
; modelParameters_g . mRTWModifiedTimeStamp = 5.91976676E+8 ;
modelParameters_g . mZcDisabled = false ; tmp_e = 0.001 ; modelParameters_g .
mSolverTolerance = tmp_e ; tmp_e = 0.0 ; modelParameters_g . mFixedStepSize =
tmp_e ; tmp_p = true ; modelParameters_g . mVariableStepSolver = tmp_p ;
tmp_p = false ; modelParameters_g . mIsUsingODEN = tmp_p ; tmp_p =
slIsRapidAcceleratorSimulating ( ) ; val = ssGetGlobalInitialStatesAvailable
( rtS ) ; if ( tmp_p ) { val = ( val && ssIsFirstInitCond ( rtS ) ) ; }
modelParameters_g . mLoadInitialState = val ; modelParameters_g . mZcDisabled
= false ; diagnosticManager = ( NeuDiagnosticManager * ) rtDW . cdslitainn ;
diagnosticTree_c = neu_diagnostic_manager_get_initial_tree (
diagnosticManager ) ; i = nesl_initialize_simulator ( ( NeslSimulator * )
rtDW . fw4oj40z44 , & modelParameters_g , diagnosticManager ) ; if ( i != 0 )
{ tmp_p = error_buffer_is_empty ( ssGetErrorStatus ( rtS ) ) ; if ( tmp_p ) {
msg_c = rtw_diagnostics_msg ( diagnosticTree_c ) ; ssSetErrorStatus ( rtS ,
msg_c ) ; } } MdlInitialize ( ) ; } void MdlOutputs ( int_T tid ) { real_T
cksfancqmu ; __m128d tmp_ck ; __m128d tmp_dz ; __m128d tmp_fs ;
NeslSimulationData * simulationData ; NeuDiagnosticManager *
diagnosticManager ; NeuDiagnosticTree * diagnosticTree ; NeuDiagnosticTree *
diagnosticTree_e ; NeuDiagnosticTree * diagnosticTree_g ; NeuDiagnosticTree *
diagnosticTree_i ; NeuDiagnosticTree * diagnosticTree_m ; NeuDiagnosticTree *
diagnosticTree_p ; alyuhn31fb * body ; alyuhn31fbf * obj_i ; alyuhn31fbfz *
obj_p ; bkyb12klys expl_temp ; carpjyrt23 * obj ; char * msg ; char * msg_e ;
char * msg_g ; char * msg_i ; char * msg_m ; char * msg_p ; e1n154mvbh * a ;
hzpfvcreu1 * Ttree ; hzpfvcreu1o * X_p ; hzpfvcreu1o * Xtree ; klgljlxbmf * S
; klgljlxbmf * aB ; klgljlxbmf * b ; klgljlxbmf * e ; klgljlxbmf * f ;
klgljlxbmf * vB ; klgljlxbmf * vJ ; klgljlxbmf * y ; lbiyaleu3n * A ;
lbiyaleu3n * s ; mjv2tzlyyi * obj_e ; mjv2tzlyyig * robot ; ot4uovzkt0w *
joint ; real_T tmp_e [ 52 ] ; real_T tmp [ 40 ] ; real_T XDHOffset [ 36 ] ;
real_T tempR_p [ 36 ] ; real_T T1 [ 16 ] ; real_T T1_p [ 16 ] ; real_T
drvc3knqcj [ 16 ] ; real_T out [ 16 ] ; real_T R [ 9 ] ; real_T tempR [ 9 ] ;
real_T tmp_l [ 9 ] ; real_T a0 [ 6 ] ; real_T avi5g2zns4 [ 6 ] ; real_T
gszrbwtx2d [ 6 ] ; real_T jnyryq4kwv [ 6 ] ; real_T locqfeuxtm_data [ 6 ] ;
real_T qddoti_data [ 6 ] ; real_T tmp_b [ 6 ] ; real_T tmp_j [ 6 ] ; real_T
vJ_p [ 6 ] ; real_T result_data [ 4 ] ; real_T tmp_c [ 4 ] ; real_T tmp_m [ 4
] ; real_T kcfpuyzftg [ 3 ] ; real_T b_idx_0 ; real_T ed5ohpi45w_idx_0 ;
real_T ed5ohpi45w_idx_1 ; real_T ed5ohpi45w_idx_2 ; real_T ed5ohpi45w_idx_4 ;
real_T ed5ohpi45w_idx_5 ; real_T enjq30wyaa ; real_T g1ewb34kax_idx_0 ;
real_T g1ewb34kax_idx_1 ; real_T jvo4w5mh0q_idx_0 ; real_T jvo4w5mh0q_idx_1 ;
real_T jvo4w5mh0q_idx_2 ; real_T jvo4w5mh0q_idx_3 ; real_T k ; real_T
kmjofsvir4 ; real_T lwk1iknpkj ; real_T m ; real_T n ; real_T n1tv2iovp0 ;
real_T t ; real_T time ; real_T time_b ; real_T time_c ; real_T time_e ;
real_T time_f ; real_T time_g ; real_T time_i ; real_T time_j ; real_T time_k
; real_T time_m ; real_T time_n ; real_T time_p ; int32_T aoffset ; int32_T
b_jcol ; int32_T iacol ; int32_T ibcol ; int32_T inner ; int32_T loop_ub ;
int32_T loop_ub_p ; int32_T ntilecols ; int32_T p ; int_T tmp_i [ 12 ] ;
int_T tmp_p [ 11 ] ; int_T tmp_f [ 3 ] ; int_T tmp_n [ 3 ] ; int_T tmp_g [ 2
] ; int_T tmp_k [ 2 ] ; char_T b_varargout_2_Status_data [ 14 ] ; char_T b_e
[ 12 ] ; char_T b_i [ 8 ] ; char_T b_p [ 5 ] ; int8_T tmp_d ; int8_T tmp_o ;
boolean_T b_bool ; static const int8_T tmp_f2 [ 16 ] = { 1 , 0 , 0 , 0 , 0 ,
1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 } ; static const char_T tmp_kt [ 5
] = { 'f' , 'i' , 'x' , 'e' , 'd' } ; static const char_T tmp_lt [ 12 ] = {
'e' , 'n' , 'd' , '_' , 'e' , 'f' , 'f' , 'e' , 'c' , 't' , 'o' , 'r' } ;
static const char_T tmp_hn [ 8 ] = { 'r' , 'e' , 'v' , 'o' , 'l' , 'u' , 't'
, 'e' } ; static const char_T tmp_ap [ 5 ] = { 'w' , 'o' , 'r' , 'l' , 'd' }
; int32_T b_varargout_2_Status_size [ 2 ] ; int32_T exitg1 ; int32_T
locqfeuxtm_size ; boolean_T exitg2 ; { real_T * pDataValues = ( real_T * )
rtDW . om1scnv1yi . DataPtr ; real_T * pTimeValues = ( real_T * ) rtDW .
om1scnv1yi . TimePtr ; int_T currTimeIndex = rtDW . l1cob02vt3 . PrevIndex ;
real_T t = ssGetTaskTime ( rtS , 0 ) ; int numPoints , lastPoint ; FWksInfo *
fromwksInfo = ( FWksInfo * ) rtDW . om1scnv1yi . RSimInfoPtr ; numPoints =
fromwksInfo -> nDataPoints ; lastPoint = numPoints - 1 ; if ( t <=
pTimeValues [ 0 ] ) { currTimeIndex = 0 ; } else if ( t >= pTimeValues [
lastPoint ] ) { currTimeIndex = lastPoint - 1 ; } else { if ( t < pTimeValues
[ currTimeIndex ] ) { while ( t < pTimeValues [ currTimeIndex ] ) {
currTimeIndex -- ; } } else { while ( t >= pTimeValues [ currTimeIndex + 1 ]
) { currTimeIndex ++ ; } } } rtDW . l1cob02vt3 . PrevIndex = currTimeIndex ;
{ real_T t1 = pTimeValues [ currTimeIndex ] ; real_T t2 = pTimeValues [
currTimeIndex + 1 ] ; if ( t1 == t2 ) { if ( t < t1 ) { cksfancqmu =
pDataValues [ currTimeIndex ] ; } else { cksfancqmu = pDataValues [
currTimeIndex + 1 ] ; } } else { real_T f1 = ( t2 - t ) / ( t2 - t1 ) ;
real_T f2 = 1.0 - f1 ; real_T d1 ; real_T d2 ; int_T TimeIndex =
currTimeIndex ; d1 = pDataValues [ TimeIndex ] ; d2 = pDataValues [ TimeIndex
+ 1 ] ; cksfancqmu = ( real_T ) rtInterpolate ( d1 , d2 , f1 , f2 ) ;
pDataValues += numPoints ; } } } kcfpuyzftg [ 0 ] = 0.28 ; kcfpuyzftg [ 1 ] =
0.0 ; kcfpuyzftg [ 2 ] = 0.2 ; if ( ! ( cksfancqmu > 1.0 ) ) { if ( rtP .
linearline_Value == 0.0 ) { m = ( 10.0 * muDoubleScalarPower ( cksfancqmu ,
3.0 ) - 15.0 * muDoubleScalarPower ( cksfancqmu , 4.0 ) ) + 6.0 *
muDoubleScalarPower ( cksfancqmu , 5.0 ) ; kcfpuyzftg [ 0 ] = - 0.28 * m +
0.28 ; kcfpuyzftg [ 1 ] = 0.28 * m ; kcfpuyzftg [ 2 ] = - 0.1 * m + 0.2 ; }
else if ( rtP . linearline_Value != 0.0 ) { kcfpuyzftg [ 0 ] = ( 1.0 -
cksfancqmu ) * 0.28 + 0.0 * cksfancqmu ; kcfpuyzftg [ 1 ] = ( 1.0 -
cksfancqmu ) * 0.0 + 0.28 * cksfancqmu ; kcfpuyzftg [ 2 ] = ( 1.0 -
cksfancqmu ) * 0.2 + 0.1 * cksfancqmu ; } } memset ( & T1 [ 0 ] , 0 , sizeof
( real_T ) << 4U ) ; T1 [ 0 ] = 1.0 ; T1 [ 5 ] = 1.0 ; T1 [ 10 ] = 1.0 ; T1 [
15 ] = 1.0 ; for ( b_jcol = 0 ; b_jcol < 4 ; b_jcol ++ ) { iacol = ( b_jcol
<< 2 ) - 1 ; ibcol = ( b_jcol << 2 ) - 1 ; out [ ibcol + 1 ] = T1 [ iacol + 1
] ; out [ ibcol + 2 ] = T1 [ iacol + 2 ] ; out [ ibcol + 3 ] = T1 [ iacol + 3
] ; out [ ibcol + 4 ] = T1 [ iacol + 4 ] ; } out [ 12 ] = kcfpuyzftg [ 0 ] ;
out [ 13 ] = kcfpuyzftg [ 1 ] ; out [ 14 ] = kcfpuyzftg [ 2 ] ; if (
ssIsSampleHit ( rtS , 1 , 0 ) ) { if ( ssIsSampleHit ( rtS , 2 , 0 ) ) { for
( iacol = 0 ; iacol < 6 ; iacol ++ ) { rtB . ojtckfkjyo [ iacol ] = rtDW .
g4g3vrgle4 [ iacol ] ; } } for ( iacol = 0 ; iacol < 6 ; iacol ++ ) { rtB .
f1tyt5vcvt [ iacol ] = rtB . ojtckfkjyo [ iacol ] ; } } obj = & rtDW .
ggw1b1lkek . IKInternal ; calykgtvmm ( & b , 1 ) ; m2n1niwoq4 ( & a , 2 ) ;
if ( rtDW . ggw1b1lkek . IKInternal . isInitialized != 1 ) { rtDW .
ggw1b1lkek . IKInternal . isSetupComplete = false ; rtDW . ggw1b1lkek .
IKInternal . isInitialized = 1 ; n = rtDW . ggw1b1lkek . IKInternal .
RigidBodyTreeInternal -> PositionNumber ; fv2pu0tbva ( & A , 2 ) ; inner = A
-> size [ 0 ] * A -> size [ 1 ] ; A -> size [ 0 ] = ( int32_T ) n ; A -> size
[ 1 ] = ( int32_T ) ( 2.0 * n ) ; n10lmcng5l ( A , inner ) ; loop_ub = (
int32_T ) ( 2.0 * n ) * ( int32_T ) n ; if ( loop_ub - 1 >= 0 ) { memset ( &
A -> data [ 0 ] , 0 , ( uint32_T ) loop_ub * sizeof ( int8_T ) ) ; } inner =
b -> size [ 0 ] ; b -> size [ 0 ] = ( int32_T ) ( 2.0 * n ) ; miyl1rq4uj ( b
, inner ) ; loop_ub = ( int32_T ) ( 2.0 * n ) ; if ( loop_ub - 1 >= 0 ) {
memset ( & b -> data [ 0 ] , 0 , ( uint32_T ) loop_ub * sizeof ( real_T ) ) ;
} k = 1.0 ; m = 1.0 ; t = rtDW . ggw1b1lkek . IKInternal .
RigidBodyTreeInternal -> NumBodies ; iacol = ( int32_T ) t - 1 ; if ( (
int32_T ) t - 1 >= 0 ) { for ( inner = 0 ; inner < 5 ; inner ++ ) { b_p [
inner ] = tmp_kt [ inner ] ; } } calykgtvmm ( & e , 2 ) ; fv2pu0tbva ( & s ,
2 ) ; for ( b_jcol = 0 ; b_jcol <= iacol ; b_jcol ++ ) { obj_p = obj ->
RigidBodyTreeInternal -> Bodies [ b_jcol ] ; joint = obj_p -> JointInternal ;
n = joint -> PositionNumber ; inner = a -> size [ 0 ] * a -> size [ 1 ] ; a
-> size [ 0 ] = 1 ; a -> size [ 1 ] = joint -> Type -> size [ 1 ] ;
e3n3mnwk4b ( a , inner ) ; loop_ub = joint -> Type -> size [ 1 ] ; for (
inner = 0 ; inner < loop_ub ; inner ++ ) { a -> data [ inner ] = joint ->
Type -> data [ inner ] ; } b_bool = false ; if ( a -> size [ 1 ] != 5 ) { }
else { ntilecols = 1 ; do { exitg1 = 0 ; if ( ntilecols - 1 < 5 ) { if ( a ->
data [ ntilecols - 1 ] != b_p [ ntilecols - 1 ] ) { exitg1 = 1 ; } else {
ntilecols ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 ==
0 ) ; } if ( ! b_bool ) { if ( k > ( k + n ) - 1.0 ) { aoffset = 0 ; } else {
aoffset = ( int32_T ) k - 1 ; } if ( m > ( m + n ) - 1.0 ) { p = 0 ; } else {
p = ( int32_T ) m - 1 ; } if ( n < 0.0 ) { t = 0.0 ; b_idx_0 = 0.0 ; } else {
t = n ; b_idx_0 = n ; } ibcol = ( int32_T ) b_idx_0 - 1 ; inner = s -> size [
0 ] * s -> size [ 1 ] ; s -> size [ 0 ] = ( int32_T ) t ; s -> size [ 1 ] = (
int32_T ) t ; n10lmcng5l ( s , inner ) ; loop_ub = ( int32_T ) t * ( int32_T
) t ; if ( loop_ub - 1 >= 0 ) { memset ( & s -> data [ 0 ] , 0 , ( uint32_T )
loop_ub * sizeof ( int8_T ) ) ; } if ( ( int32_T ) t > 0 ) { for ( ntilecols
= 0 ; ntilecols <= ibcol ; ntilecols ++ ) { s -> data [ ntilecols + s -> size
[ 0 ] * ntilecols ] = 1 ; } } loop_ub = s -> size [ 1 ] ; for ( inner = 0 ;
inner < loop_ub ; inner ++ ) { loop_ub_p = s -> size [ 0 ] ; for ( ntilecols
= 0 ; ntilecols < loop_ub_p ; ntilecols ++ ) { A -> data [ ( aoffset +
ntilecols ) + A -> size [ 0 ] * ( p + inner ) ] = s -> data [ s -> size [ 0 ]
* inner + ntilecols ] ; } } if ( k > ( k + n ) - 1.0 ) { p = 0 ; } else { p =
( int32_T ) k - 1 ; } t = m + n ; if ( t > ( 2.0 * n + m ) - 1.0 ) { aoffset
= 0 ; } else { aoffset = ( int32_T ) t - 1 ; } if ( n < 0.0 ) { t = 0.0 ; }
else { t = n ; } inner = s -> size [ 0 ] * s -> size [ 1 ] ; s -> size [ 0 ]
= ( int32_T ) t ; s -> size [ 1 ] = ( int32_T ) t ; n10lmcng5l ( s , inner )
; loop_ub = ( int32_T ) t * ( int32_T ) t ; if ( loop_ub - 1 >= 0 ) { memset
( & s -> data [ 0 ] , 0 , ( uint32_T ) loop_ub * sizeof ( int8_T ) ) ; } if (
( int32_T ) t > 0 ) { for ( ntilecols = 0 ; ntilecols <= ibcol ; ntilecols ++
) { s -> data [ ntilecols + s -> size [ 0 ] * ntilecols ] = 1 ; } } loop_ub =
s -> size [ 1 ] ; for ( inner = 0 ; inner < loop_ub ; inner ++ ) { loop_ub_p
= s -> size [ 0 ] ; for ( ntilecols = 0 ; ntilecols < loop_ub_p ; ntilecols
++ ) { A -> data [ ( p + ntilecols ) + A -> size [ 0 ] * ( aoffset + inner )
] = ( int8_T ) - s -> data [ s -> size [ 0 ] * inner + ntilecols ] ; } }
inner = e -> size [ 0 ] * e -> size [ 1 ] ; e -> size [ 0 ] = joint ->
PositionLimitsInternal -> size [ 0 ] ; e -> size [ 1 ] = 2 ; miyl1rq4uj ( e ,
inner ) ; loop_ub = joint -> PositionLimitsInternal -> size [ 0 ] << 1 ; for
( inner = 0 ; inner < loop_ub ; inner ++ ) { e -> data [ inner ] = joint ->
PositionLimitsInternal -> data [ inner ] ; } b -> data [ ( int32_T ) m - 1 ]
= e -> data [ 1 ] ; inner = e -> size [ 0 ] * e -> size [ 1 ] ; e -> size [ 0
] = joint -> PositionLimitsInternal -> size [ 0 ] ; e -> size [ 1 ] = 2 ;
miyl1rq4uj ( e , inner ) ; loop_ub = joint -> PositionLimitsInternal -> size
[ 0 ] << 1 ; for ( inner = 0 ; inner < loop_ub ; inner ++ ) { e -> data [
inner ] = joint -> PositionLimitsInternal -> data [ inner ] ; } b -> data [ (
int32_T ) ( m + 1.0 ) - 1 ] = - e -> data [ 0 ] ; m += 2.0 * n ; } k += n ; }
bqvhlmvfpz ( & s ) ; bdhoypqe5x ( & e ) ; inner = rtDW . ggw1b1lkek .
IKInternal . Solver -> ConstraintMatrix -> size [ 0 ] * rtDW . ggw1b1lkek .
IKInternal . Solver -> ConstraintMatrix -> size [ 1 ] ; rtDW . ggw1b1lkek .
IKInternal . Solver -> ConstraintMatrix -> size [ 0 ] = A -> size [ 0 ] ;
rtDW . ggw1b1lkek . IKInternal . Solver -> ConstraintMatrix -> size [ 1 ] = A
-> size [ 1 ] ; miyl1rq4uj ( rtDW . ggw1b1lkek . IKInternal . Solver ->
ConstraintMatrix , inner ) ; loop_ub = A -> size [ 0 ] * A -> size [ 1 ] ;
for ( ibcol = 0 ; ibcol < loop_ub ; ibcol ++ ) { rtDW . ggw1b1lkek .
IKInternal . Solver -> ConstraintMatrix -> data [ ibcol ] = A -> data [ ibcol
] ; } bqvhlmvfpz ( & A ) ; inner = rtDW . ggw1b1lkek . IKInternal . Solver ->
ConstraintBound -> size [ 0 ] ; rtDW . ggw1b1lkek . IKInternal . Solver ->
ConstraintBound -> size [ 0 ] = b -> size [ 0 ] ; miyl1rq4uj ( rtDW .
ggw1b1lkek . IKInternal . Solver -> ConstraintBound , inner ) ; loop_ub = b
-> size [ 0 ] ; for ( inner = 0 ; inner < loop_ub ; inner ++ ) { rtDW .
ggw1b1lkek . IKInternal . Solver -> ConstraintBound -> data [ inner ] = b ->
data [ inner ] ; } lij5c2bvo0 ( rtDW . ggw1b1lkek . IKInternal .
RigidBodyTreeInternal , rtDW . ggw1b1lkek . IKInternal . Limits ) ; obj ->
_pobj0 . matlabCodegenIsDeleted = false ; rtDW . ggw1b1lkek . IKInternal .
Solver -> ExtraArgs = & obj -> _pobj0 ; for ( inner = 0 ; inner < 36 ; inner
++ ) { rtDW . ggw1b1lkek . IKInternal . Solver -> ExtraArgs -> WeightMatrix [
inner ] = 0.0 ; } rtDW . ggw1b1lkek . IKInternal . Solver -> ExtraArgs ->
Robot = rtDW . ggw1b1lkek . IKInternal . RigidBodyTreeInternal ; memset ( &
T1 [ 0 ] , 0 , sizeof ( real_T ) << 4U ) ; T1 [ 0 ] = 1.0 ; T1 [ 5 ] = 1.0 ;
T1 [ 10 ] = 1.0 ; T1 [ 15 ] = 1.0 ; for ( inner = 0 ; inner < 16 ; inner ++ )
{ rtDW . ggw1b1lkek . IKInternal . Solver -> ExtraArgs -> Tform [ inner ] =
T1 [ inner ] ; } rtDW . ggw1b1lkek . IKInternal . Solver -> ExtraArgs ->
BodyName -> size [ 0 ] = 1 ; rtDW . ggw1b1lkek . IKInternal . Solver ->
ExtraArgs -> BodyName -> size [ 1 ] = 0 ; inner = rtDW . ggw1b1lkek .
IKInternal . Solver -> ExtraArgs -> ErrTemp -> size [ 0 ] ; rtDW . ggw1b1lkek
. IKInternal . Solver -> ExtraArgs -> ErrTemp -> size [ 0 ] = 6 ; miyl1rq4uj
( rtDW . ggw1b1lkek . IKInternal . Solver -> ExtraArgs -> ErrTemp , inner ) ;
for ( inner = 0 ; inner < 6 ; inner ++ ) { rtDW . ggw1b1lkek . IKInternal .
Solver -> ExtraArgs -> ErrTemp -> data [ inner ] = 0.0 ; } rtDW . ggw1b1lkek
. IKInternal . Solver -> ExtraArgs -> CostTemp = 0.0 ; inner = b -> size [ 0
] ; b -> size [ 0 ] = ( int32_T ) rtDW . ggw1b1lkek . IKInternal .
RigidBodyTreeInternal -> PositionNumber ; miyl1rq4uj ( b , inner ) ; loop_ub
= ( int32_T ) rtDW . ggw1b1lkek . IKInternal . RigidBodyTreeInternal ->
PositionNumber ; if ( loop_ub - 1 >= 0 ) { memset ( & b -> data [ 0 ] , 0 , (
uint32_T ) loop_ub * sizeof ( real_T ) ) ; } inner = rtDW . ggw1b1lkek .
IKInternal . Solver -> ExtraArgs -> GradTemp -> size [ 0 ] ; rtDW .
ggw1b1lkek . IKInternal . Solver -> ExtraArgs -> GradTemp -> size [ 0 ] = b
-> size [ 0 ] ; miyl1rq4uj ( rtDW . ggw1b1lkek . IKInternal . Solver ->
ExtraArgs -> GradTemp , inner ) ; loop_ub = b -> size [ 0 ] ; for ( inner = 0
; inner < loop_ub ; inner ++ ) { rtDW . ggw1b1lkek . IKInternal . Solver ->
ExtraArgs -> GradTemp -> data [ inner ] = 0.0 ; } rtDW . ggw1b1lkek .
IKInternal . isSetupComplete = true ; } gai2oub5e3 ( & rtDW . ggw1b1lkek .
IKInternal , out , rtP . IKWeights_Value , rtB . f1tyt5vcvt , rtB .
lo0hqyptao , & rtB . gyuiwjkgbx , & k , & n , b_varargout_2_Status_data ,
b_varargout_2_Status_size ) ; if ( ssIsSampleHit ( rtS , 1 , 0 ) ) { obj_e =
& rtDW . c2ho3h3oyv . TreeInternal ; n = rtDW . c2ho3h3oyv . TreeInternal .
NumBodies ; for ( inner = 0 ; inner < 16 ; inner ++ ) { expl_temp . f1 [
inner ] = tmp_f2 [ inner ] ; } jarne05rzi ( & Ttree , 2 ) ; inner = Ttree ->
size [ 0 ] * Ttree -> size [ 1 ] ; Ttree -> size [ 0 ] = 1 ; Ttree -> size [
1 ] = ( int32_T ) n ; f33velrxf0 ( Ttree , inner ) ; if ( ( int32_T ) n != 0
) { ntilecols = ( int32_T ) n - 1 ; for ( b_jcol = 0 ; b_jcol <= ntilecols ;
b_jcol ++ ) { Ttree -> data [ b_jcol ] = expl_temp ; } } k = 1.0 ; ibcol = (
int32_T ) n - 1 ; if ( ( int32_T ) n - 1 >= 0 ) { for ( inner = 0 ; inner < 5
; inner ++ ) { b_p [ inner ] = tmp_kt [ inner ] ; } } for ( b_jcol = 0 ;
b_jcol <= ibcol ; b_jcol ++ ) { body = obj_e -> Bodies [ b_jcol ] ; n = body
-> JointInternal . PositionNumber ; t = ( k + n ) - 1.0 ; if ( k > t ) { p =
0 ; iacol = 0 ; } else { p = ( int32_T ) k - 1 ; iacol = ( int32_T ) t ; }
for ( inner = 0 ; inner < 16 ; inner ++ ) { T1 [ inner ] = body ->
JointInternal . JointToParentTransform [ inner ] ; } inner = a -> size [ 0 ]
* a -> size [ 1 ] ; a -> size [ 0 ] = 1 ; a -> size [ 1 ] = body ->
JointInternal . Type -> size [ 1 ] ; e3n3mnwk4b ( a , inner ) ; loop_ub =
body -> JointInternal . Type -> size [ 1 ] ; for ( inner = 0 ; inner <
loop_ub ; inner ++ ) { a -> data [ inner ] = body -> JointInternal . Type ->
data [ inner ] ; } b_bool = false ; if ( a -> size [ 1 ] != 5 ) { } else {
ntilecols = 1 ; do { exitg1 = 0 ; if ( ntilecols - 1 < 5 ) { if ( a -> data [
ntilecols - 1 ] != b_p [ ntilecols - 1 ] ) { exitg1 = 1 ; } else { ntilecols
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { m = 0.0 ; } else { for ( inner = 0 ; inner < 8 ; inner ++ ) {
b_i [ inner ] = tmp_hn [ inner ] ; } if ( a -> size [ 1 ] != 8 ) { } else {
ntilecols = 1 ; do { exitg1 = 0 ; if ( ntilecols - 1 < 8 ) { if ( a -> data [
ntilecols - 1 ] != b_i [ ntilecols - 1 ] ) { exitg1 = 1 ; } else { ntilecols
++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if
( b_bool ) { m = 1.0 ; } else { m = - 1.0 ; } } switch ( ( int32_T ) m ) {
case 0 : memset ( & out [ 0 ] , 0 , sizeof ( real_T ) << 4U ) ; out [ 0 ] =
1.0 ; out [ 5 ] = 1.0 ; out [ 10 ] = 1.0 ; out [ 15 ] = 1.0 ; break ; case 1
: cnixr4pd3a ( & body -> JointInternal , kcfpuyzftg ) ; result_data [ 0 ] =
kcfpuyzftg [ 0 ] ; result_data [ 1 ] = kcfpuyzftg [ 1 ] ; result_data [ 2 ] =
kcfpuyzftg [ 2 ] ; if ( ( iacol - p != 0 ) - 1 >= 0 ) { result_data [ 3 ] =
rtB . ojtckfkjyo [ p ] ; } m = result_data [ 0 ] ; kcfpuyzftg [ 0 ] = m * m ;
m = result_data [ 1 ] ; kcfpuyzftg [ 1 ] = m * m ; m = result_data [ 2 ] ; m
= 1.0 / muDoubleScalarSqrt ( ( kcfpuyzftg [ 0 ] + kcfpuyzftg [ 1 ] ) + m * m
) ; kcfpuyzftg [ 0 ] = result_data [ 0 ] * m ; kcfpuyzftg [ 1 ] = result_data
[ 1 ] * m ; kcfpuyzftg [ 2 ] = result_data [ 2 ] * m ; t = result_data [ 3 ]
; m = muDoubleScalarCos ( t ) ; t = muDoubleScalarSin ( t ) ; tempR [ 0 ] =
kcfpuyzftg [ 0 ] * kcfpuyzftg [ 0 ] * ( 1.0 - m ) + m ; tempR [ 1 ] =
kcfpuyzftg [ 0 ] * kcfpuyzftg [ 1 ] * ( 1.0 - m ) - kcfpuyzftg [ 2 ] * t ;
tempR [ 2 ] = kcfpuyzftg [ 0 ] * kcfpuyzftg [ 2 ] * ( 1.0 - m ) + kcfpuyzftg
[ 1 ] * t ; tempR [ 3 ] = kcfpuyzftg [ 0 ] * kcfpuyzftg [ 1 ] * ( 1.0 - m ) +
kcfpuyzftg [ 2 ] * t ; tempR [ 4 ] = kcfpuyzftg [ 1 ] * kcfpuyzftg [ 1 ] * (
1.0 - m ) + m ; tempR [ 5 ] = kcfpuyzftg [ 1 ] * kcfpuyzftg [ 2 ] * ( 1.0 - m
) - kcfpuyzftg [ 0 ] * t ; tempR [ 6 ] = kcfpuyzftg [ 0 ] * kcfpuyzftg [ 2 ]
* ( 1.0 - m ) - kcfpuyzftg [ 1 ] * t ; tempR [ 7 ] = kcfpuyzftg [ 1 ] *
kcfpuyzftg [ 2 ] * ( 1.0 - m ) + kcfpuyzftg [ 0 ] * t ; tempR [ 8 ] =
kcfpuyzftg [ 2 ] * kcfpuyzftg [ 2 ] * ( 1.0 - m ) + m ; for ( ntilecols = 0 ;
ntilecols < 3 ; ntilecols ++ ) { R [ ntilecols ] = tempR [ ntilecols * 3 ] ;
R [ ntilecols + 3 ] = tempR [ ntilecols * 3 + 1 ] ; R [ ntilecols + 6 ] =
tempR [ ntilecols * 3 + 2 ] ; } memset ( & out [ 0 ] , 0 , sizeof ( real_T )
<< 4U ) ; for ( inner = 0 ; inner < 3 ; inner ++ ) { out [ inner << 2 ] = R [
3 * inner ] ; out [ ( inner << 2 ) + 1 ] = R [ 3 * inner + 1 ] ; out [ (
inner << 2 ) + 2 ] = R [ 3 * inner + 2 ] ; } out [ 15 ] = 1.0 ; break ;
default : cnixr4pd3a ( & body -> JointInternal , kcfpuyzftg ) ; memset ( &
tempR [ 0 ] , 0 , 9U * sizeof ( real_T ) ) ; tempR [ 0 ] = 1.0 ; tempR [ 4 ]
= 1.0 ; tempR [ 8 ] = 1.0 ; b_idx_0 = rtB . ojtckfkjyo [ p ] ; for ( inner =
0 ; inner < 3 ; inner ++ ) { out [ inner << 2 ] = tempR [ 3 * inner ] ; out [
( inner << 2 ) + 1 ] = tempR [ 3 * inner + 1 ] ; out [ ( inner << 2 ) + 2 ] =
tempR [ 3 * inner + 2 ] ; out [ inner + 12 ] = kcfpuyzftg [ inner ] * b_idx_0
; } out [ 3 ] = 0.0 ; out [ 7 ] = 0.0 ; out [ 11 ] = 0.0 ; out [ 15 ] = 1.0 ;
break ; } for ( inner = 0 ; inner < 16 ; inner ++ ) { drvc3knqcj [ inner ] =
body -> JointInternal . ChildToJointTransform [ inner ] ; } for ( inner = 0 ;
inner < 4 ; inner ++ ) { for ( ntilecols = 0 ; ntilecols < 4 ; ntilecols ++ )
{ T1_p [ inner + ( ntilecols << 2 ) ] = 0.0 ; T1_p [ inner + ( ntilecols << 2
) ] += out [ ntilecols << 2 ] * T1 [ inner ] ; T1_p [ inner + ( ntilecols <<
2 ) ] += out [ ( ntilecols << 2 ) + 1 ] * T1 [ inner + 4 ] ; T1_p [ inner + (
ntilecols << 2 ) ] += out [ ( ntilecols << 2 ) + 2 ] * T1 [ inner + 8 ] ;
T1_p [ inner + ( ntilecols << 2 ) ] += out [ ( ntilecols << 2 ) + 3 ] * T1 [
inner + 12 ] ; } for ( ntilecols = 0 ; ntilecols < 4 ; ntilecols ++ ) { Ttree
-> data [ b_jcol ] . f1 [ inner + ( ntilecols << 2 ) ] = 0.0 ; Ttree -> data
[ b_jcol ] . f1 [ inner + ( ntilecols << 2 ) ] = Ttree -> data [ b_jcol ] .
f1 [ ( ntilecols << 2 ) + inner ] + drvc3knqcj [ ntilecols << 2 ] * T1_p [
inner ] ; Ttree -> data [ b_jcol ] . f1 [ inner + ( ntilecols << 2 ) ] =
drvc3knqcj [ ( ntilecols << 2 ) + 1 ] * T1_p [ inner + 4 ] + Ttree -> data [
b_jcol ] . f1 [ ( ntilecols << 2 ) + inner ] ; Ttree -> data [ b_jcol ] . f1
[ inner + ( ntilecols << 2 ) ] = drvc3knqcj [ ( ntilecols << 2 ) + 2 ] * T1_p
[ inner + 8 ] + Ttree -> data [ b_jcol ] . f1 [ ( ntilecols << 2 ) + inner ]
; Ttree -> data [ b_jcol ] . f1 [ inner + ( ntilecols << 2 ) ] = drvc3knqcj [
( ntilecols << 2 ) + 3 ] * T1_p [ inner + 12 ] + Ttree -> data [ b_jcol ] .
f1 [ ( ntilecols << 2 ) + inner ] ; } } k += n ; if ( body -> ParentIndex >
0.0 ) { for ( inner = 0 ; inner < 16 ; inner ++ ) { T1 [ inner ] = Ttree ->
data [ ( int32_T ) body -> ParentIndex - 1 ] . f1 [ inner ] ; } for ( inner =
0 ; inner < 4 ; inner ++ ) { for ( ntilecols = 0 ; ntilecols < 4 ; ntilecols
++ ) { T1_p [ inner + ( ntilecols << 2 ) ] = 0.0 ; T1_p [ inner + ( ntilecols
<< 2 ) ] += Ttree -> data [ b_jcol ] . f1 [ ntilecols << 2 ] * T1 [ inner ] ;
T1_p [ inner + ( ntilecols << 2 ) ] += Ttree -> data [ b_jcol ] . f1 [ (
ntilecols << 2 ) + 1 ] * T1 [ inner + 4 ] ; T1_p [ inner + ( ntilecols << 2 )
] += Ttree -> data [ b_jcol ] . f1 [ ( ntilecols << 2 ) + 2 ] * T1 [ inner +
8 ] ; T1_p [ inner + ( ntilecols << 2 ) ] += Ttree -> data [ b_jcol ] . f1 [
( ntilecols << 2 ) + 3 ] * T1 [ inner + 12 ] ; } } memcpy ( & Ttree -> data [
b_jcol ] . f1 [ 0 ] , & T1_p [ 0 ] , sizeof ( real_T ) << 4U ) ; } } k = -
1.0 ; inner = a -> size [ 0 ] * a -> size [ 1 ] ; a -> size [ 0 ] = 1 ; a ->
size [ 1 ] = obj_e -> Base . NameInternal -> size [ 1 ] ; e3n3mnwk4b ( a ,
inner ) ; loop_ub = obj_e -> Base . NameInternal -> size [ 1 ] ; for ( inner
= 0 ; inner < loop_ub ; inner ++ ) { a -> data [ inner ] = obj_e -> Base .
NameInternal -> data [ inner ] ; } for ( inner = 0 ; inner < 12 ; inner ++ )
{ b_e [ inner ] = tmp_lt [ inner ] ; } b_bool = false ; if ( a -> size [ 1 ]
!= 12 ) { } else { ntilecols = 1 ; do { exitg1 = 0 ; if ( ntilecols - 1 < 12
) { if ( a -> data [ ntilecols - 1 ] != b_e [ ntilecols - 1 ] ) { exitg1 = 1
; } else { ntilecols ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while (
exitg1 == 0 ) ; } if ( b_bool ) { k = 0.0 ; } else { m = rtDW . c2ho3h3oyv .
TreeInternal . NumBodies ; b_jcol = 0 ; exitg2 = false ; while ( ( ! exitg2 )
&& ( b_jcol <= ( int32_T ) m - 1 ) ) { body = obj_e -> Bodies [ b_jcol ] ;
inner = a -> size [ 0 ] * a -> size [ 1 ] ; a -> size [ 0 ] = 1 ; a -> size [
1 ] = body -> NameInternal -> size [ 1 ] ; e3n3mnwk4b ( a , inner ) ; loop_ub
= body -> NameInternal -> size [ 1 ] ; for ( inner = 0 ; inner < loop_ub ;
inner ++ ) { a -> data [ inner ] = body -> NameInternal -> data [ inner ] ; }
for ( inner = 0 ; inner < 12 ; inner ++ ) { b_e [ inner ] = tmp_lt [ inner ]
; } if ( a -> size [ 1 ] != 12 ) { } else { ntilecols = 1 ; do { exitg1 = 0 ;
if ( ntilecols - 1 < 12 ) { if ( a -> data [ ntilecols - 1 ] != b_e [
ntilecols - 1 ] ) { exitg1 = 1 ; } else { ntilecols ++ ; } } else { b_bool =
true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { k = (
real_T ) b_jcol + 1.0 ; exitg2 = true ; } else { b_jcol ++ ; } } } if ( k ==
0.0 ) { memset ( & T1 [ 0 ] , 0 , sizeof ( real_T ) << 4U ) ; T1 [ 0 ] = 1.0
; T1 [ 5 ] = 1.0 ; T1 [ 10 ] = 1.0 ; T1 [ 15 ] = 1.0 ; } else { for ( inner =
0 ; inner < 16 ; inner ++ ) { T1 [ inner ] = Ttree -> data [ ( int32_T ) k -
1 ] . f1 [ inner ] ; } } k = - 1.0 ; inner = a -> size [ 0 ] * a -> size [ 1
] ; a -> size [ 0 ] = 1 ; a -> size [ 1 ] = obj_e -> Base . NameInternal ->
size [ 1 ] ; e3n3mnwk4b ( a , inner ) ; loop_ub = obj_e -> Base .
NameInternal -> size [ 1 ] ; for ( inner = 0 ; inner < loop_ub ; inner ++ ) {
a -> data [ inner ] = obj_e -> Base . NameInternal -> data [ inner ] ; } for
( inner = 0 ; inner < 5 ; inner ++ ) { b_p [ inner ] = tmp_ap [ inner ] ; }
b_bool = false ; if ( a -> size [ 1 ] != 5 ) { } else { ntilecols = 1 ; do {
exitg1 = 0 ; if ( ntilecols - 1 < 5 ) { if ( a -> data [ ntilecols - 1 ] !=
b_p [ ntilecols - 1 ] ) { exitg1 = 1 ; } else { ntilecols ++ ; } } else {
b_bool = true ; exitg1 = 1 ; } } while ( exitg1 == 0 ) ; } if ( b_bool ) { k
= 0.0 ; } else { m = rtDW . c2ho3h3oyv . TreeInternal . NumBodies ; b_jcol =
0 ; exitg2 = false ; while ( ( ! exitg2 ) && ( b_jcol <= ( int32_T ) m - 1 )
) { body = obj_e -> Bodies [ b_jcol ] ; inner = a -> size [ 0 ] * a -> size [
1 ] ; a -> size [ 0 ] = 1 ; a -> size [ 1 ] = body -> NameInternal -> size [
1 ] ; e3n3mnwk4b ( a , inner ) ; loop_ub = body -> NameInternal -> size [ 1 ]
; for ( inner = 0 ; inner < loop_ub ; inner ++ ) { a -> data [ inner ] = body
-> NameInternal -> data [ inner ] ; } for ( inner = 0 ; inner < 5 ; inner ++
) { b_p [ inner ] = tmp_ap [ inner ] ; } if ( a -> size [ 1 ] != 5 ) { } else
{ ntilecols = 1 ; do { exitg1 = 0 ; if ( ntilecols - 1 < 5 ) { if ( a -> data
[ ntilecols - 1 ] != b_p [ ntilecols - 1 ] ) { exitg1 = 1 ; } else {
ntilecols ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 ==
0 ) ; } if ( b_bool ) { k = ( real_T ) b_jcol + 1.0 ; exitg2 = true ; } else
{ b_jcol ++ ; } } } if ( k == 0.0 ) { memset ( & out [ 0 ] , 0 , sizeof (
real_T ) << 4U ) ; out [ 0 ] = 1.0 ; out [ 5 ] = 1.0 ; out [ 10 ] = 1.0 ; out
[ 15 ] = 1.0 ; } else { for ( inner = 0 ; inner < 16 ; inner ++ ) { out [
inner ] = Ttree -> data [ ( int32_T ) k - 1 ] . f1 [ inner ] ; } } jhl11noygj
( & Ttree ) ; for ( inner = 0 ; inner < 3 ; inner ++ ) { R [ 3 * inner ] =
out [ inner ] ; R [ 3 * inner + 1 ] = out [ inner + 4 ] ; R [ 3 * inner + 2 ]
= out [ inner + 8 ] ; } for ( inner = 0 ; inner < 9 ; inner ++ ) { tempR [
inner ] = - R [ inner ] ; } for ( inner = 0 ; inner < 3 ; inner ++ ) { T1_p [
inner << 2 ] = R [ 3 * inner ] ; T1_p [ ( inner << 2 ) + 1 ] = R [ 3 * inner
+ 1 ] ; T1_p [ ( inner << 2 ) + 2 ] = R [ 3 * inner + 2 ] ; T1_p [ inner + 12
] = ( tempR [ inner + 3 ] * out [ 13 ] + tempR [ inner ] * out [ 12 ] ) +
tempR [ inner + 6 ] * out [ 14 ] ; } T1_p [ 3 ] = 0.0 ; T1_p [ 7 ] = 0.0 ;
T1_p [ 11 ] = 0.0 ; T1_p [ 15 ] = 1.0 ; for ( inner = 0 ; inner < 4 ; inner
++ ) { for ( ntilecols = 0 ; ntilecols <= 2 ; ntilecols += 2 ) {
_mm_storeu_pd ( & drvc3knqcj [ ntilecols + ( inner << 2 ) ] , _mm_set1_pd (
0.0 ) ) ; tmp_fs = _mm_loadu_pd ( & T1_p [ ntilecols ] ) ; tmp_ck =
_mm_loadu_pd ( & drvc3knqcj [ ( inner << 2 ) + ntilecols ] ) ; _mm_storeu_pd
( & drvc3knqcj [ ntilecols + ( inner << 2 ) ] , _mm_add_pd ( tmp_ck ,
_mm_mul_pd ( _mm_set1_pd ( T1 [ inner << 2 ] ) , tmp_fs ) ) ) ; tmp_fs =
_mm_loadu_pd ( & T1_p [ ntilecols + 4 ] ) ; tmp_ck = _mm_loadu_pd ( &
drvc3knqcj [ ( inner << 2 ) + ntilecols ] ) ; _mm_storeu_pd ( & drvc3knqcj [
ntilecols + ( inner << 2 ) ] , _mm_add_pd ( _mm_mul_pd ( _mm_set1_pd ( T1 [ (
inner << 2 ) + 1 ] ) , tmp_fs ) , tmp_ck ) ) ; tmp_fs = _mm_loadu_pd ( & T1_p
[ ntilecols + 8 ] ) ; tmp_ck = _mm_loadu_pd ( & drvc3knqcj [ ( inner << 2 ) +
ntilecols ] ) ; _mm_storeu_pd ( & drvc3knqcj [ ntilecols + ( inner << 2 ) ] ,
_mm_add_pd ( _mm_mul_pd ( _mm_set1_pd ( T1 [ ( inner << 2 ) + 2 ] ) , tmp_fs
) , tmp_ck ) ) ; tmp_fs = _mm_loadu_pd ( & T1_p [ ntilecols + 12 ] ) ; tmp_ck
= _mm_loadu_pd ( & drvc3knqcj [ ( inner << 2 ) + ntilecols ] ) ;
_mm_storeu_pd ( & drvc3knqcj [ ntilecols + ( inner << 2 ) ] , _mm_add_pd (
_mm_mul_pd ( _mm_set1_pd ( T1 [ ( inner << 2 ) + 3 ] ) , tmp_fs ) , tmp_ck )
) ; } } rtB . fs4h3j1l31 [ 0 ] = drvc3knqcj [ 12 ] / drvc3knqcj [ 15 ] ; rtB
. fs4h3j1l31 [ 1 ] = drvc3knqcj [ 13 ] / drvc3knqcj [ 15 ] ; rtB . fs4h3j1l31
[ 2 ] = drvc3knqcj [ 14 ] / drvc3knqcj [ 15 ] ; } simulationData = (
NeslSimulationData * ) rtDW . a5qgeff343 ; time = ssGetT ( rtS ) ;
simulationData -> mData -> mTime . mN = 1 ; simulationData -> mData -> mTime
. mX = & time ; simulationData -> mData -> mContStates . mN = 12 ;
simulationData -> mData -> mContStates . mX = & rtX . ge5rtzzo4y [ 0 ] ;
simulationData -> mData -> mDiscStates . mN = 0 ; simulationData -> mData ->
mDiscStates . mX = & rtDW . ofd1yxgqn2 ; simulationData -> mData ->
mModeVector . mN = 0 ; simulationData -> mData -> mModeVector . mX = & rtDW .
edj0t2oelw ; b_bool = ( ssIsMajorTimeStep ( rtS ) && ssGetRTWSolverInfo ( rtS
) -> foundContZcEvents ) ; simulationData -> mData -> mFoundZcEvents = b_bool
; simulationData -> mData -> mIsMajorTimeStep = ssIsMajorTimeStep ( rtS ) ;
b_bool = ( ssGetMdlInfoPtr ( rtS ) -> mdlFlags . solverAssertCheck == 1U ) ;
simulationData -> mData -> mIsSolverAssertCheck = b_bool ; b_bool =
ssIsSolverCheckingCIC ( rtS ) ; simulationData -> mData ->
mIsSolverCheckingCIC = b_bool ; b_bool = ssIsSolverComputingJacobian ( rtS )
; simulationData -> mData -> mIsComputingJacobian = b_bool ; simulationData
-> mData -> mIsEvaluatingF0 = ( ssGetEvaluatingF0ForJacobian ( rtS ) != 0 ) ;
b_bool = ssIsSolverRequestingReset ( rtS ) ; simulationData -> mData ->
mIsSolverRequestingReset = b_bool ; simulationData -> mData ->
mIsModeUpdateTimeStep = ssIsModeUpdateTimeStep ( rtS ) ; tmp_p [ 0 ] = 0 ;
tmp [ 0 ] = rtB . b5i1gadrfg [ 0 ] ; tmp [ 1 ] = rtB . b5i1gadrfg [ 1 ] ; tmp
[ 2 ] = rtB . b5i1gadrfg [ 2 ] ; tmp [ 3 ] = rtB . b5i1gadrfg [ 3 ] ; tmp_p [
1 ] = 4 ; tmp [ 4 ] = rtB . o4mrrxpgqr [ 0 ] ; tmp [ 5 ] = rtB . o4mrrxpgqr [
1 ] ; tmp [ 6 ] = rtB . o4mrrxpgqr [ 2 ] ; tmp [ 7 ] = rtB . o4mrrxpgqr [ 3 ]
; tmp_p [ 2 ] = 8 ; tmp [ 8 ] = rtB . bpxoa5jqph [ 0 ] ; tmp [ 9 ] = rtB .
bpxoa5jqph [ 1 ] ; tmp [ 10 ] = rtB . bpxoa5jqph [ 2 ] ; tmp [ 11 ] = rtB .
bpxoa5jqph [ 3 ] ; tmp_p [ 3 ] = 12 ; tmp [ 12 ] = rtB . gtqltersrn [ 0 ] ;
tmp [ 13 ] = rtB . gtqltersrn [ 1 ] ; tmp [ 14 ] = rtB . gtqltersrn [ 2 ] ;
tmp [ 15 ] = rtB . gtqltersrn [ 3 ] ; tmp_p [ 4 ] = 16 ; tmp [ 16 ] = rtB .
kfmkqerxag [ 0 ] ; tmp [ 17 ] = rtB . kfmkqerxag [ 1 ] ; tmp [ 18 ] = rtB .
kfmkqerxag [ 2 ] ; tmp [ 19 ] = rtB . kfmkqerxag [ 3 ] ; tmp_p [ 5 ] = 20 ;
tmp [ 20 ] = rtB . kqkimrfdm5 [ 0 ] ; tmp [ 21 ] = rtB . kqkimrfdm5 [ 1 ] ;
tmp [ 22 ] = rtB . kqkimrfdm5 [ 2 ] ; tmp [ 23 ] = rtB . kqkimrfdm5 [ 3 ] ;
tmp_p [ 6 ] = 24 ; tmp [ 24 ] = rtB . ffuvz5uef3 [ 0 ] ; tmp [ 25 ] = rtB .
ffuvz5uef3 [ 1 ] ; tmp [ 26 ] = rtB . ffuvz5uef3 [ 2 ] ; tmp [ 27 ] = rtB .
ffuvz5uef3 [ 3 ] ; tmp_p [ 7 ] = 28 ; tmp [ 28 ] = rtB . j5luahqacu [ 0 ] ;
tmp [ 29 ] = rtB . j5luahqacu [ 1 ] ; tmp [ 30 ] = rtB . j5luahqacu [ 2 ] ;
tmp [ 31 ] = rtB . j5luahqacu [ 3 ] ; tmp_p [ 8 ] = 32 ; tmp [ 32 ] = rtB .
ecaqpvxc3x [ 0 ] ; tmp [ 33 ] = rtB . ecaqpvxc3x [ 1 ] ; tmp [ 34 ] = rtB .
ecaqpvxc3x [ 2 ] ; tmp [ 35 ] = rtB . ecaqpvxc3x [ 3 ] ; tmp_p [ 9 ] = 36 ;
tmp [ 36 ] = rtB . d5kmywozm5 [ 0 ] ; tmp [ 37 ] = rtB . d5kmywozm5 [ 1 ] ;
tmp [ 38 ] = rtB . d5kmywozm5 [ 2 ] ; tmp [ 39 ] = rtB . d5kmywozm5 [ 3 ] ;
tmp_p [ 10 ] = 40 ; simulationData -> mData -> mInputValues . mN = 40 ;
simulationData -> mData -> mInputValues . mX = & tmp [ 0 ] ; simulationData
-> mData -> mInputOffsets . mN = 11 ; simulationData -> mData ->
mInputOffsets . mX = & tmp_p [ 0 ] ; simulationData -> mData -> mOutputs . mN
= 12 ; simulationData -> mData -> mOutputs . mX = & rtB . iuhvwu1nwu [ 0 ] ;
simulationData -> mData -> mTolerances . mN = 0 ; simulationData -> mData ->
mTolerances . mX = NULL ; simulationData -> mData -> mCstateHasChanged =
false ; time_p = ssGetTaskTime ( rtS , 0 ) ; simulationData -> mData -> mTime
. mN = 1 ; simulationData -> mData -> mTime . mX = & time_p ; simulationData
-> mData -> mSampleHits . mN = 0 ; simulationData -> mData -> mSampleHits .
mX = NULL ; simulationData -> mData -> mIsFundamentalSampleHit = false ;
diagnosticManager = ( NeuDiagnosticManager * ) rtDW . caiz5lqz5f ;
diagnosticTree = neu_diagnostic_manager_get_initial_tree ( diagnosticManager
) ; ibcol = ne_simulator_method ( ( NeslSimulator * ) rtDW . etcerwyaoj ,
NESL_SIM_OUTPUTS , simulationData , diagnosticManager ) ; if ( ibcol != 0 ) {
b_bool = error_buffer_is_empty ( ssGetErrorStatus ( rtS ) ) ; if ( b_bool ) {
msg = rtw_diagnostics_msg ( diagnosticTree ) ; ssSetErrorStatus ( rtS , msg )
; } } if ( ssIsMajorTimeStep ( rtS ) && simulationData -> mData ->
mCstateHasChanged ) { ssSetBlockStateForSolverChangedAtMajorStep ( rtS ) ; }
simulationData = ( NeslSimulationData * ) rtDW . papxjgo3op ; time_e = ssGetT
( rtS ) ; simulationData -> mData -> mTime . mN = 1 ; simulationData -> mData
-> mTime . mX = & time_e ; simulationData -> mData -> mContStates . mN = 0 ;
simulationData -> mData -> mContStates . mX = NULL ; simulationData -> mData
-> mDiscStates . mN = 0 ; simulationData -> mData -> mDiscStates . mX = &
rtDW . imqlsd2301 ; simulationData -> mData -> mModeVector . mN = 0 ;
simulationData -> mData -> mModeVector . mX = & rtDW . arxdbmacvj ; b_bool =
( ssIsMajorTimeStep ( rtS ) && ssGetRTWSolverInfo ( rtS ) ->
foundContZcEvents ) ; simulationData -> mData -> mFoundZcEvents = b_bool ;
simulationData -> mData -> mIsMajorTimeStep = ssIsMajorTimeStep ( rtS ) ;
b_bool = ( ssGetMdlInfoPtr ( rtS ) -> mdlFlags . solverAssertCheck == 1U ) ;
simulationData -> mData -> mIsSolverAssertCheck = b_bool ; b_bool =
ssIsSolverCheckingCIC ( rtS ) ; simulationData -> mData ->
mIsSolverCheckingCIC = b_bool ; simulationData -> mData ->
mIsComputingJacobian = false ; simulationData -> mData -> mIsEvaluatingF0 =
false ; b_bool = ssIsSolverRequestingReset ( rtS ) ; simulationData -> mData
-> mIsSolverRequestingReset = b_bool ; simulationData -> mData ->
mIsModeUpdateTimeStep = ssIsModeUpdateTimeStep ( rtS ) ; tmp_i [ 0 ] = 0 ;
tmp_e [ 0 ] = rtB . b5i1gadrfg [ 0 ] ; tmp_e [ 1 ] = rtB . b5i1gadrfg [ 1 ] ;
tmp_e [ 2 ] = rtB . b5i1gadrfg [ 2 ] ; tmp_e [ 3 ] = rtB . b5i1gadrfg [ 3 ] ;
tmp_i [ 1 ] = 4 ; tmp_e [ 4 ] = rtB . o4mrrxpgqr [ 0 ] ; tmp_e [ 5 ] = rtB .
o4mrrxpgqr [ 1 ] ; tmp_e [ 6 ] = rtB . o4mrrxpgqr [ 2 ] ; tmp_e [ 7 ] = rtB .
o4mrrxpgqr [ 3 ] ; tmp_i [ 2 ] = 8 ; tmp_e [ 8 ] = rtB . bpxoa5jqph [ 0 ] ;
tmp_e [ 9 ] = rtB . bpxoa5jqph [ 1 ] ; tmp_e [ 10 ] = rtB . bpxoa5jqph [ 2 ]
; tmp_e [ 11 ] = rtB . bpxoa5jqph [ 3 ] ; tmp_i [ 3 ] = 12 ; tmp_e [ 12 ] =
rtB . gtqltersrn [ 0 ] ; tmp_e [ 13 ] = rtB . gtqltersrn [ 1 ] ; tmp_e [ 14 ]
= rtB . gtqltersrn [ 2 ] ; tmp_e [ 15 ] = rtB . gtqltersrn [ 3 ] ; tmp_i [ 4
] = 16 ; tmp_e [ 16 ] = rtB . kfmkqerxag [ 0 ] ; tmp_e [ 17 ] = rtB .
kfmkqerxag [ 1 ] ; tmp_e [ 18 ] = rtB . kfmkqerxag [ 2 ] ; tmp_e [ 19 ] = rtB
. kfmkqerxag [ 3 ] ; tmp_i [ 5 ] = 20 ; tmp_e [ 20 ] = rtB . kqkimrfdm5 [ 0 ]
; tmp_e [ 21 ] = rtB . kqkimrfdm5 [ 1 ] ; tmp_e [ 22 ] = rtB . kqkimrfdm5 [ 2
] ; tmp_e [ 23 ] = rtB . kqkimrfdm5 [ 3 ] ; tmp_i [ 6 ] = 24 ; tmp_e [ 24 ] =
rtB . ffuvz5uef3 [ 0 ] ; tmp_e [ 25 ] = rtB . ffuvz5uef3 [ 1 ] ; tmp_e [ 26 ]
= rtB . ffuvz5uef3 [ 2 ] ; tmp_e [ 27 ] = rtB . ffuvz5uef3 [ 3 ] ; tmp_i [ 7
] = 28 ; tmp_e [ 28 ] = rtB . j5luahqacu [ 0 ] ; tmp_e [ 29 ] = rtB .
j5luahqacu [ 1 ] ; tmp_e [ 30 ] = rtB . j5luahqacu [ 2 ] ; tmp_e [ 31 ] = rtB
. j5luahqacu [ 3 ] ; tmp_i [ 8 ] = 32 ; tmp_e [ 32 ] = rtB . ecaqpvxc3x [ 0 ]
; tmp_e [ 33 ] = rtB . ecaqpvxc3x [ 1 ] ; tmp_e [ 34 ] = rtB . ecaqpvxc3x [ 2
] ; tmp_e [ 35 ] = rtB . ecaqpvxc3x [ 3 ] ; tmp_i [ 9 ] = 36 ; tmp_e [ 36 ] =
rtB . d5kmywozm5 [ 0 ] ; tmp_e [ 37 ] = rtB . d5kmywozm5 [ 1 ] ; tmp_e [ 38 ]
= rtB . d5kmywozm5 [ 2 ] ; tmp_e [ 39 ] = rtB . d5kmywozm5 [ 3 ] ; tmp_i [ 10
] = 40 ; memcpy ( & tmp_e [ 40 ] , & rtB . iuhvwu1nwu [ 0 ] , 12U * sizeof (
real_T ) ) ; tmp_i [ 11 ] = 52 ; simulationData -> mData -> mInputValues . mN
= 52 ; simulationData -> mData -> mInputValues . mX = & tmp_e [ 0 ] ;
simulationData -> mData -> mInputOffsets . mN = 12 ; simulationData -> mData
-> mInputOffsets . mX = & tmp_i [ 0 ] ; simulationData -> mData -> mOutputs .
mN = 8 ; simulationData -> mData -> mOutputs . mX = & rtB . ev24cemjyk [ 0 ]
; simulationData -> mData -> mTolerances . mN = 0 ; simulationData -> mData
-> mTolerances . mX = NULL ; simulationData -> mData -> mCstateHasChanged =
false ; time_i = ssGetTaskTime ( rtS , 0 ) ; simulationData -> mData -> mTime
. mN = 1 ; simulationData -> mData -> mTime . mX = & time_i ; simulationData
-> mData -> mSampleHits . mN = 0 ; simulationData -> mData -> mSampleHits .
mX = NULL ; simulationData -> mData -> mIsFundamentalSampleHit = false ;
diagnosticManager = ( NeuDiagnosticManager * ) rtDW . auudttxoyr ;
diagnosticTree_p = neu_diagnostic_manager_get_initial_tree (
diagnosticManager ) ; ibcol = ne_simulator_method ( ( NeslSimulator * ) rtDW
. oibswdkijz , NESL_SIM_OUTPUTS , simulationData , diagnosticManager ) ; if (
ibcol != 0 ) { b_bool = error_buffer_is_empty ( ssGetErrorStatus ( rtS ) ) ;
if ( b_bool ) { msg_p = rtw_diagnostics_msg ( diagnosticTree_p ) ;
ssSetErrorStatus ( rtS , msg_p ) ; } } if ( ssIsMajorTimeStep ( rtS ) &&
simulationData -> mData -> mCstateHasChanged ) {
ssSetBlockStateForSolverChangedAtMajorStep ( rtS ) ; } if ( ssIsSampleHit (
rtS , 1 , 0 ) && ssIsSampleHit ( rtS , 2 , 0 ) ) { rtDW . jqx3umoi0g [ 0 ] =
rtB . ev24cemjyk [ 2 ] ; rtDW . jqx3umoi0g [ 1 ] = rtB . ev24cemjyk [ 3 ] ;
rtDW . jqx3umoi0g [ 2 ] = rtB . ev24cemjyk [ 4 ] ; rtDW . jqx3umoi0g [ 3 ] =
rtB . ev24cemjyk [ 5 ] ; rtDW . jqx3umoi0g [ 4 ] = rtB . ev24cemjyk [ 0 ] ;
rtDW . jqx3umoi0g [ 5 ] = rtB . ev24cemjyk [ 1 ] ; } if ( ssIsSampleHit ( rtS
, 2 , 0 ) ) { for ( iacol = 0 ; iacol < 6 ; iacol ++ ) { rtB . hhdb535tvz [
iacol ] = rtDW . jqx3umoi0g [ iacol ] ; } } if ( ssIsSampleHit ( rtS , 1 , 0
) ) { { if ( rtDW . mdhjjuxljs . AQHandles && ssGetLogOutput ( rtS ) ) {
sdiWriteSignal ( rtDW . mdhjjuxljs . AQHandles , ssGetTaskTime ( rtS , 1 ) ,
( char * ) & rtB . fs4h3j1l31 [ 0 ] + 0 ) ; } } { if ( rtDW . h42mla3rja .
AQHandles && ssGetLogOutput ( rtS ) ) { sdiWriteSignal ( rtDW . h42mla3rja .
AQHandles , ssGetTaskTime ( rtS , 1 ) , ( char * ) & rtB . gyuiwjkgbx + 0 ) ;
} } } if ( ssIsSampleHit ( rtS , 2 , 0 ) ) { { if ( rtDW . knk4hpmg1v .
AQHandles && ssGetLogOutput ( rtS ) ) { sdiWriteSignal ( rtDW . knk4hpmg1v .
AQHandles , ssGetTaskTime ( rtS , 2 ) , ( char * ) & rtB . hhdb535tvz [ 0 ] +
0 ) ; } } for ( inner = 0 ; inner <= 4 ; inner += 2 ) { tmp_fs = _mm_loadu_pd
( & rtDW . jiqh55j12p [ inner ] ) ; _mm_storeu_pd ( & avi5g2zns4 [ inner ] ,
_mm_mul_pd ( _mm_set1_pd ( rtP . OutputFilter_NumCoef ) , tmp_fs ) ) ; }
b_idx_0 = rtP . InputRateLimiter_RisingLim * nf31gr4fkh ; m = rtP .
InputRateLimiter_RisingLim * nf31gr4fkh ; n = rtP .
InputRateLimiter_FallingLim * nf31gr4fkh ; k = rtP .
InputRateLimiter_FallingLim * nf31gr4fkh ; n1tv2iovp0 = rtP .
InputFilter_NumCoef * rtDW . ahio0mgx5h [ 0 ] ; jvo4w5mh0q_idx_3 = n1tv2iovp0
- rtDW . bwwbiride4 [ 0 ] ; if ( jvo4w5mh0q_idx_3 > b_idx_0 ) {
jvo4w5mh0q_idx_3 = rtDW . bwwbiride4 [ 0 ] + m ; } else if ( jvo4w5mh0q_idx_3
< n ) { jvo4w5mh0q_idx_3 = rtDW . bwwbiride4 [ 0 ] + k ; } else {
jvo4w5mh0q_idx_3 = n1tv2iovp0 ; } rtDW . bwwbiride4 [ 0 ] = jvo4w5mh0q_idx_3
; jvo4w5mh0q_idx_0 = jvo4w5mh0q_idx_3 ; n1tv2iovp0 = rtP .
InputFilter_NumCoef * rtDW . ahio0mgx5h [ 1 ] ; jvo4w5mh0q_idx_3 = n1tv2iovp0
- rtDW . bwwbiride4 [ 1 ] ; if ( jvo4w5mh0q_idx_3 > b_idx_0 ) {
jvo4w5mh0q_idx_3 = rtDW . bwwbiride4 [ 1 ] + m ; } else if ( jvo4w5mh0q_idx_3
< n ) { jvo4w5mh0q_idx_3 = rtDW . bwwbiride4 [ 1 ] + k ; } else {
jvo4w5mh0q_idx_3 = n1tv2iovp0 ; } rtDW . bwwbiride4 [ 1 ] = jvo4w5mh0q_idx_3
; jvo4w5mh0q_idx_1 = jvo4w5mh0q_idx_3 ; n1tv2iovp0 = rtP .
InputFilter_NumCoef * rtDW . ahio0mgx5h [ 2 ] ; jvo4w5mh0q_idx_3 = n1tv2iovp0
- rtDW . bwwbiride4 [ 2 ] ; if ( jvo4w5mh0q_idx_3 > b_idx_0 ) {
jvo4w5mh0q_idx_3 = rtDW . bwwbiride4 [ 2 ] + m ; } else if ( jvo4w5mh0q_idx_3
< n ) { jvo4w5mh0q_idx_3 = rtDW . bwwbiride4 [ 2 ] + k ; } else {
jvo4w5mh0q_idx_3 = n1tv2iovp0 ; } rtDW . bwwbiride4 [ 2 ] = jvo4w5mh0q_idx_3
; jvo4w5mh0q_idx_2 = jvo4w5mh0q_idx_3 ; n1tv2iovp0 = rtP .
InputFilter_NumCoef * rtDW . ahio0mgx5h [ 3 ] ; jvo4w5mh0q_idx_3 = n1tv2iovp0
- rtDW . bwwbiride4 [ 3 ] ; if ( jvo4w5mh0q_idx_3 > b_idx_0 ) {
jvo4w5mh0q_idx_3 = rtDW . bwwbiride4 [ 3 ] + m ; } else if ( jvo4w5mh0q_idx_3
< n ) { jvo4w5mh0q_idx_3 = rtDW . bwwbiride4 [ 3 ] + k ; } else {
jvo4w5mh0q_idx_3 = n1tv2iovp0 ; } rtDW . bwwbiride4 [ 3 ] = jvo4w5mh0q_idx_3
; n1tv2iovp0 = jvo4w5mh0q_idx_0 - rtB . hhdb535tvz [ 0 ] ; rtB . dmuwptdi5m =
( rtP . Kd1 * n1tv2iovp0 - rtDW . jmlpsyjpqa ) * rtP . Kfilt1 ; n1tv2iovp0 =
rtP . Kp1 * n1tv2iovp0 + rtB . dmuwptdi5m ; lwk1iknpkj = jvo4w5mh0q_idx_1 -
rtB . hhdb535tvz [ 1 ] ; rtB . crue1g0m5e = ( rtP . Kd2 * lwk1iknpkj - rtDW .
osgjdwjesl ) * rtP . Kfilt2 ; lwk1iknpkj = rtP . Kp2 * lwk1iknpkj + rtB .
crue1g0m5e ; kmjofsvir4 = jvo4w5mh0q_idx_2 - rtB . hhdb535tvz [ 2 ] ; rtB .
oolxi5a4g3 = ( rtP . Kd3 * kmjofsvir4 - rtDW . fkcgzk5oma ) * rtP . Kfilt3 ;
kmjofsvir4 = rtP . Kp3 * kmjofsvir4 + rtB . oolxi5a4g3 ; enjq30wyaa =
jvo4w5mh0q_idx_3 - rtB . hhdb535tvz [ 3 ] ; rtB . kl0afniunm = ( rtP . Kd4 *
enjq30wyaa - rtDW . c45aidngho ) * rtP . Kfilt4 ; enjq30wyaa = rtP . Kp4 *
enjq30wyaa + rtB . kl0afniunm ; } if ( ssIsSampleHit ( rtS , 1 , 0 ) &&
ssIsSampleHit ( rtS , 2 , 0 ) ) { rtDW . axm3re1vxp [ 0 ] = rtB . lo0hqyptao
[ 0 ] ; rtDW . axm3re1vxp [ 1 ] = rtB . lo0hqyptao [ 1 ] ; rtDW . axm3re1vxp
[ 2 ] = rtB . lo0hqyptao [ 2 ] ; rtDW . axm3re1vxp [ 3 ] = rtB . lo0hqyptao [
3 ] ; rtDW . axm3re1vxp [ 4 ] = rtP . Constant3_Value ; } if ( ssIsSampleHit
( rtS , 2 , 0 ) ) { for ( iacol = 0 ; iacol < 5 ; iacol ++ ) { rtB .
kj5qpxedtg [ iacol ] = rtDW . axm3re1vxp [ iacol ] ; } if ( rtB . kj5qpxedtg
[ 4 ] > rtP . Switch1_Threshold ) { g1ewb34kax_idx_0 = rtP .
ClosedGripperPosition_Value [ 0 ] ; g1ewb34kax_idx_1 = rtP .
ClosedGripperPosition_Value [ 1 ] ; } else { g1ewb34kax_idx_0 = rtP .
OpenGripperPosition_Value [ 0 ] ; g1ewb34kax_idx_1 = rtP .
OpenGripperPosition_Value [ 1 ] ; } m = g1ewb34kax_idx_0 - rtB . hhdb535tvz [
4 ] ; t = rtP . KpGrip * m + rtDW . kaentdmtdn ; k = g1ewb34kax_idx_1 - rtB .
hhdb535tvz [ 5 ] ; n = rtP . KpGrip * k + rtDW . d5wogx3kqa ;
ed5ohpi45w_idx_0 = avi5g2zns4 [ 0 ] + n1tv2iovp0 ; ed5ohpi45w_idx_1 =
avi5g2zns4 [ 1 ] + lwk1iknpkj ; ed5ohpi45w_idx_2 = avi5g2zns4 [ 2 ] +
kmjofsvir4 ; enjq30wyaa += avi5g2zns4 [ 3 ] ; if ( t > rtP . gripUpperLimit )
{ b_idx_0 = rtP . gripUpperLimit ; } else if ( t < rtP . gripLowerLimit ) {
b_idx_0 = rtP . gripLowerLimit ; } else { b_idx_0 = t ; } ed5ohpi45w_idx_4 =
avi5g2zns4 [ 4 ] + b_idx_0 ; if ( n > rtP . gripUpperLimit ) { b_idx_0 = rtP
. gripUpperLimit ; } else if ( n < rtP . gripLowerLimit ) { b_idx_0 = rtP .
gripLowerLimit ; } else { b_idx_0 = n ; } ed5ohpi45w_idx_5 = avi5g2zns4 [ 5 ]
+ b_idx_0 ; avi5g2zns4 [ 0 ] = jvo4w5mh0q_idx_0 ; avi5g2zns4 [ 1 ] =
jvo4w5mh0q_idx_1 ; avi5g2zns4 [ 2 ] = jvo4w5mh0q_idx_2 ; avi5g2zns4 [ 3 ] =
jvo4w5mh0q_idx_3 ; avi5g2zns4 [ 4 ] = g1ewb34kax_idx_0 ; avi5g2zns4 [ 5 ] =
g1ewb34kax_idx_1 ; for ( iacol = 0 ; iacol <= 4 ; iacol += 2 ) { tmp_fs =
_mm_loadu_pd ( & avi5g2zns4 [ iacol ] ) ; _mm_storeu_pd ( & rtB . nqkvctls4x
[ iacol ] , _mm_mul_pd ( tmp_fs , _mm_set1_pd ( rtP . TSamp_WtEt ) ) ) ;
tmp_fs = _mm_loadu_pd ( & rtB . nqkvctls4x [ iacol ] ) ; tmp_ck =
_mm_loadu_pd ( & rtDW . ggzlanf4wa [ iacol ] ) ; tmp_fs = _mm_sub_pd ( tmp_fs
, tmp_ck ) ; _mm_storeu_pd ( & rtB . gn1fk0rbkj [ iacol ] , _mm_mul_pd (
tmp_fs , _mm_set1_pd ( rtP . TSamp_WtEt_d4bsp0r1mg ) ) ) ; tmp_ck =
_mm_loadu_pd ( & rtB . gn1fk0rbkj [ iacol ] ) ; tmp_dz = _mm_loadu_pd ( &
rtDW . p4ux5yfv5l [ iacol ] ) ; _mm_storeu_pd ( & gszrbwtx2d [ iacol ] ,
_mm_sub_pd ( tmp_ck , tmp_dz ) ) ; _mm_storeu_pd ( & jnyryq4kwv [ iacol ] ,
tmp_fs ) ; } robot = & rtDW . pqz301f1cc . TreeInternal ; a0 [ 0 ] = 0.0 ; a0
[ 1 ] = 0.0 ; a0 [ 2 ] = 0.0 ; a0 [ 3 ] = - rtDW . pqz301f1cc . TreeInternal
. Gravity [ 0 ] ; a0 [ 4 ] = - rtDW . pqz301f1cc . TreeInternal . Gravity [ 1
] ; a0 [ 5 ] = - rtDW . pqz301f1cc . TreeInternal . Gravity [ 2 ] ;
n1tv2iovp0 = rtDW . pqz301f1cc . TreeInternal . NumBodies ; calykgtvmm ( & vJ
, 2 ) ; inner = vJ -> size [ 0 ] * vJ -> size [ 1 ] ; vJ -> size [ 0 ] = 6 ;
vJ -> size [ 1 ] = ( int32_T ) n1tv2iovp0 ; miyl1rq4uj ( vJ , inner ) ;
loop_ub = 6 * ( int32_T ) n1tv2iovp0 ; if ( loop_ub - 1 >= 0 ) { memset ( &
vJ -> data [ 0 ] , 0 , ( uint32_T ) loop_ub * sizeof ( real_T ) ) ; }
calykgtvmm ( & vB , 2 ) ; inner = vB -> size [ 0 ] * vB -> size [ 1 ] ; vB ->
size [ 0 ] = 6 ; vB -> size [ 1 ] = ( int32_T ) n1tv2iovp0 ; miyl1rq4uj ( vB
, inner ) ; loop_ub = 6 * ( int32_T ) n1tv2iovp0 ; if ( loop_ub - 1 >= 0 ) {
memset ( & vB -> data [ 0 ] , 0 , ( uint32_T ) loop_ub * sizeof ( real_T ) )
; } calykgtvmm ( & aB , 2 ) ; inner = aB -> size [ 0 ] * aB -> size [ 1 ] ;
aB -> size [ 0 ] = 6 ; aB -> size [ 1 ] = ( int32_T ) n1tv2iovp0 ; miyl1rq4uj
( aB , inner ) ; loop_ub = 6 * ( int32_T ) n1tv2iovp0 ; if ( loop_ub - 1 >= 0
) { memset ( & aB -> data [ 0 ] , 0 , ( uint32_T ) loop_ub * sizeof ( real_T
) ) ; } for ( iacol = 0 ; iacol < 6 ; iacol ++ ) { rtB . k3szni1qeu [ iacol ]
= 0.0 ; } b_jcol = ( int32_T ) n1tv2iovp0 - 1 ; cdtxuhnnn2 ( & Xtree , 2 ) ;
inner = Xtree -> size [ 0 ] * Xtree -> size [ 1 ] ; Xtree -> size [ 0 ] = 1 ;
Xtree -> size [ 1 ] = ( int32_T ) n1tv2iovp0 ; obcr1nckmu ( Xtree , inner ) ;
cdtxuhnnn2 ( & X_p , 2 ) ; inner = X_p -> size [ 0 ] * X_p -> size [ 1 ] ;
X_p -> size [ 0 ] = 1 ; X_p -> size [ 1 ] = ( int32_T ) n1tv2iovp0 ;
obcr1nckmu ( X_p , inner ) ; for ( ntilecols = 0 ; ntilecols <= b_jcol ;
ntilecols ++ ) { memset ( & Xtree -> data [ ntilecols ] . f1 [ 0 ] , 0 , 36U
* sizeof ( real_T ) ) ; for ( iacol = 0 ; iacol < 6 ; iacol ++ ) { Xtree ->
data [ ntilecols ] . f1 [ iacol + 6 * iacol ] = 1.0 ; } memset ( & X_p ->
data [ ntilecols ] . f1 [ 0 ] , 0 , 36U * sizeof ( real_T ) ) ; for ( iacol =
0 ; iacol < 6 ; iacol ++ ) { X_p -> data [ ntilecols ] . f1 [ iacol + 6 *
iacol ] = 1.0 ; } } iacol = ( int32_T ) n1tv2iovp0 - 1 ; calykgtvmm ( & f , 2
) ; inner = f -> size [ 0 ] * f -> size [ 1 ] ; f -> size [ 0 ] = 6 ; f ->
size [ 1 ] = ( int32_T ) n1tv2iovp0 ; miyl1rq4uj ( f , inner ) ; if ( (
int32_T ) n1tv2iovp0 - 1 >= 0 ) { tmp_l [ 0 ] = 0.0 ; tmp_l [ 4 ] = 0.0 ;
tmp_l [ 8 ] = 0.0 ; } calykgtvmm ( & S , 2 ) ; calykgtvmm ( & y , 2 ) ; for (
b_jcol = 0 ; b_jcol <= iacol ; b_jcol ++ ) { obj_i = robot -> Bodies [ b_jcol
] ; inner = S -> size [ 0 ] * S -> size [ 1 ] ; S -> size [ 0 ] = 6 ; S ->
size [ 1 ] = obj_i -> JointInternal . MotionSubspace -> size [ 1 ] ;
miyl1rq4uj ( S , inner ) ; loop_ub = 6 * obj_i -> JointInternal .
MotionSubspace -> size [ 1 ] ; for ( inner = 0 ; inner < loop_ub ; inner ++ )
{ S -> data [ inner ] = obj_i -> JointInternal . MotionSubspace -> data [
inner ] ; } g1ewb34kax_idx_0 = robot -> PositionDoFMap [ b_jcol ] ;
g1ewb34kax_idx_1 = robot -> PositionDoFMap [ b_jcol + 9 ] ; b_idx_0 = robot
-> VelocityDoFMap [ b_jcol ] ; jvo4w5mh0q_idx_0 = robot -> VelocityDoFMap [
b_jcol + 9 ] ; memset ( & XDHOffset [ 0 ] , 0 , 36U * sizeof ( real_T ) ) ;
for ( ntilecols = 0 ; ntilecols < 6 ; ntilecols ++ ) { XDHOffset [ ntilecols
+ 6 * ntilecols ] = 1.0 ; } if ( g1ewb34kax_idx_1 < g1ewb34kax_idx_0 ) {
obj_i = robot -> Bodies [ b_jcol ] ; n4riw4ioyc ( & obj_i -> JointInternal ,
T1 ) ; qddoti_data [ 0 ] = 0.0 ; for ( inner = 0 ; inner < 6 ; inner ++ ) {
vJ -> data [ inner + 6 * b_jcol ] = 0.0 ; } } else { if ( g1ewb34kax_idx_0 >
g1ewb34kax_idx_1 ) { ntilecols = 0 ; aoffset = 0 ; } else { ntilecols = (
int32_T ) g1ewb34kax_idx_0 - 1 ; aoffset = ( int32_T ) g1ewb34kax_idx_1 ; }
if ( b_idx_0 > jvo4w5mh0q_idx_0 ) { p = - 1 ; ibcol = 0 ; inner = 0 ; } else
{ p = ( int32_T ) b_idx_0 - 2 ; ibcol = ( int32_T ) b_idx_0 - 1 ; inner = (
int32_T ) jvo4w5mh0q_idx_0 ; } loop_ub = inner - ibcol ; for ( inner = 0 ;
inner < loop_ub ; inner ++ ) { qddoti_data [ inner ] = gszrbwtx2d [ ibcol +
inner ] ; } obj_i = robot -> Bodies [ b_jcol ] ; locqfeuxtm_size = aoffset -
ntilecols ; loop_ub = aoffset - ntilecols ; for ( inner = 0 ; inner < loop_ub
; inner ++ ) { locqfeuxtm_data [ inner ] = avi5g2zns4 [ ntilecols + inner ] ;
} n4riw4ioyca ( & obj_i -> JointInternal , locqfeuxtm_data , &
locqfeuxtm_size , T1 ) ; obj_i = robot -> Bodies [ b_jcol ] ; for ( inner = 0
; inner < 16 ; inner ++ ) { out [ inner ] = obj_i -> JointInternal .
ChildToJointTransform [ inner ] ; } for ( inner = 0 ; inner < 3 ; inner ++ )
{ R [ 3 * inner ] = out [ inner ] ; R [ 3 * inner + 1 ] = out [ inner + 4 ] ;
R [ 3 * inner + 2 ] = out [ inner + 8 ] ; } for ( inner = 0 ; inner < 9 ;
inner ++ ) { tempR [ inner ] = - R [ inner ] ; } for ( inner = 0 ; inner < 3
; inner ++ ) { drvc3knqcj [ inner << 2 ] = R [ 3 * inner ] ; drvc3knqcj [ (
inner << 2 ) + 1 ] = R [ 3 * inner + 1 ] ; drvc3knqcj [ ( inner << 2 ) + 2 ]
= R [ 3 * inner + 2 ] ; drvc3knqcj [ inner + 12 ] = ( tempR [ inner + 3 ] *
out [ 13 ] + tempR [ inner ] * out [ 12 ] ) + tempR [ inner + 6 ] * out [ 14
] ; } drvc3knqcj [ 3 ] = 0.0 ; drvc3knqcj [ 7 ] = 0.0 ; drvc3knqcj [ 11 ] =
0.0 ; drvc3knqcj [ 15 ] = 1.0 ; R [ 0 ] = 0.0 ; R [ 3 ] = - drvc3knqcj [ 14 ]
; R [ 6 ] = drvc3knqcj [ 13 ] ; R [ 1 ] = drvc3knqcj [ 14 ] ; R [ 4 ] = 0.0 ;
R [ 7 ] = - drvc3knqcj [ 12 ] ; R [ 2 ] = - drvc3knqcj [ 13 ] ; R [ 5 ] =
drvc3knqcj [ 12 ] ; R [ 8 ] = 0.0 ; for ( inner = 0 ; inner < 3 ; inner ++ )
{ for ( ntilecols = 0 ; ntilecols < 3 ; ntilecols ++ ) { tempR [ inner + 3 *
ntilecols ] = 0.0 ; tempR [ inner + 3 * ntilecols ] += drvc3knqcj [ ntilecols
<< 2 ] * R [ inner ] ; tempR [ inner + 3 * ntilecols ] += drvc3knqcj [ (
ntilecols << 2 ) + 1 ] * R [ inner + 3 ] ; tempR [ inner + 3 * ntilecols ] +=
drvc3knqcj [ ( ntilecols << 2 ) + 2 ] * R [ inner + 6 ] ; XDHOffset [
ntilecols + 6 * inner ] = drvc3knqcj [ ( inner << 2 ) + ntilecols ] ;
XDHOffset [ ntilecols + 6 * ( inner + 3 ) ] = 0.0 ; } } for ( inner = 0 ;
inner < 3 ; inner ++ ) { XDHOffset [ 6 * inner + 3 ] = tempR [ 3 * inner ] ;
XDHOffset [ 6 * ( inner + 3 ) + 3 ] = drvc3knqcj [ inner << 2 ] ; XDHOffset [
6 * inner + 4 ] = tempR [ 3 * inner + 1 ] ; XDHOffset [ 6 * ( inner + 3 ) + 4
] = drvc3knqcj [ ( inner << 2 ) + 1 ] ; XDHOffset [ 6 * inner + 5 ] = tempR [
3 * inner + 2 ] ; XDHOffset [ 6 * ( inner + 3 ) + 5 ] = drvc3knqcj [ ( inner
<< 2 ) + 2 ] ; } l5ks21uytp ( XDHOffset , S , y ) ; inner = y -> size [ 1 ] -
1 ; for ( ntilecols = 0 ; ntilecols < 6 ; ntilecols ++ ) { vJ -> data [
ntilecols + 6 * b_jcol ] = 0.0 ; } for ( ntilecols = 0 ; ntilecols <= inner ;
ntilecols ++ ) { aoffset = ntilecols * 6 - 1 ; for ( ibcol = 0 ; ibcol <= 4 ;
ibcol += 2 ) { tmp_fs = _mm_loadu_pd ( & y -> data [ ( aoffset + ibcol ) + 1
] ) ; tmp_ck = _mm_loadu_pd ( & vJ -> data [ 6 * b_jcol + ibcol ] ) ;
_mm_storeu_pd ( & vJ -> data [ ibcol + 6 * b_jcol ] , _mm_add_pd ( _mm_mul_pd
( tmp_fs , _mm_set1_pd ( jnyryq4kwv [ ( p + ntilecols ) + 1 ] ) ) , tmp_ck )
) ; } } } for ( inner = 0 ; inner < 3 ; inner ++ ) { R [ 3 * inner ] = T1 [
inner ] ; R [ 3 * inner + 1 ] = T1 [ inner + 4 ] ; R [ 3 * inner + 2 ] = T1 [
inner + 8 ] ; } for ( inner = 0 ; inner < 9 ; inner ++ ) { tempR [ inner ] =
- R [ inner ] ; } for ( inner = 0 ; inner < 3 ; inner ++ ) { drvc3knqcj [
inner << 2 ] = R [ 3 * inner ] ; drvc3knqcj [ ( inner << 2 ) + 1 ] = R [ 3 *
inner + 1 ] ; drvc3knqcj [ ( inner << 2 ) + 2 ] = R [ 3 * inner + 2 ] ;
drvc3knqcj [ inner + 12 ] = ( tempR [ inner + 3 ] * T1 [ 13 ] + tempR [ inner
] * T1 [ 12 ] ) + tempR [ inner + 6 ] * T1 [ 14 ] ; } drvc3knqcj [ 3 ] = 0.0
; drvc3knqcj [ 7 ] = 0.0 ; drvc3knqcj [ 11 ] = 0.0 ; drvc3knqcj [ 15 ] = 1.0
; tmp_l [ 3 ] = - drvc3knqcj [ 14 ] ; tmp_l [ 6 ] = drvc3knqcj [ 13 ] ; tmp_l
[ 1 ] = drvc3knqcj [ 14 ] ; tmp_l [ 7 ] = - drvc3knqcj [ 12 ] ; tmp_l [ 2 ] =
- drvc3knqcj [ 13 ] ; tmp_l [ 5 ] = drvc3knqcj [ 12 ] ; for ( inner = 0 ;
inner < 3 ; inner ++ ) { for ( ntilecols = 0 ; ntilecols < 3 ; ntilecols ++ )
{ R [ inner + 3 * ntilecols ] = 0.0 ; R [ inner + 3 * ntilecols ] +=
drvc3knqcj [ ntilecols << 2 ] * tmp_l [ inner ] ; R [ inner + 3 * ntilecols ]
+= drvc3knqcj [ ( ntilecols << 2 ) + 1 ] * tmp_l [ inner + 3 ] ; R [ inner +
3 * ntilecols ] += drvc3knqcj [ ( ntilecols << 2 ) + 2 ] * tmp_l [ inner + 6
] ; X_p -> data [ b_jcol ] . f1 [ ntilecols + 6 * inner ] = drvc3knqcj [ (
inner << 2 ) + ntilecols ] ; X_p -> data [ b_jcol ] . f1 [ ntilecols + 6 * (
inner + 3 ) ] = 0.0 ; } } for ( inner = 0 ; inner < 3 ; inner ++ ) { X_p ->
data [ b_jcol ] . f1 [ 6 * inner + 3 ] = R [ 3 * inner ] ; X_p -> data [
b_jcol ] . f1 [ 6 * ( inner + 3 ) + 3 ] = drvc3knqcj [ inner << 2 ] ; X_p ->
data [ b_jcol ] . f1 [ 6 * inner + 4 ] = R [ 3 * inner + 1 ] ; X_p -> data [
b_jcol ] . f1 [ 6 * ( inner + 3 ) + 4 ] = drvc3knqcj [ ( inner << 2 ) + 1 ] ;
X_p -> data [ b_jcol ] . f1 [ 6 * inner + 5 ] = R [ 3 * inner + 2 ] ; X_p ->
data [ b_jcol ] . f1 [ 6 * ( inner + 3 ) + 5 ] = drvc3knqcj [ ( inner << 2 )
+ 2 ] ; } kmjofsvir4 = robot -> Bodies [ b_jcol ] -> ParentIndex ; if (
kmjofsvir4 > 0.0 ) { for ( inner = 0 ; inner < 6 ; inner ++ ) { b_idx_0 = 0.0
; for ( ntilecols = 0 ; ntilecols < 6 ; ntilecols ++ ) { b_idx_0 += vB ->
data [ ( ( int32_T ) kmjofsvir4 - 1 ) * 6 + ntilecols ] * X_p -> data [
b_jcol ] . f1 [ 6 * ntilecols + inner ] ; } vJ_p [ inner ] = vJ -> data [ 6 *
b_jcol + inner ] + b_idx_0 ; } for ( inner = 0 ; inner < 6 ; inner ++ ) { vB
-> data [ inner + 6 * b_jcol ] = vJ_p [ inner ] ; } l5ks21uytp ( XDHOffset ,
S , y ) ; inner = y -> size [ 1 ] - 1 ; for ( ntilecols = 0 ; ntilecols < 6 ;
ntilecols ++ ) { locqfeuxtm_data [ ntilecols ] = 0.0 ; } for ( ntilecols = 0
; ntilecols <= inner ; ntilecols ++ ) { aoffset = ntilecols * 6 - 1 ; for (
ibcol = 0 ; ibcol <= 4 ; ibcol += 2 ) { tmp_fs = _mm_loadu_pd ( & y -> data [
( aoffset + ibcol ) + 1 ] ) ; tmp_ck = _mm_loadu_pd ( & locqfeuxtm_data [
ibcol ] ) ; _mm_storeu_pd ( & locqfeuxtm_data [ ibcol ] , _mm_add_pd (
_mm_mul_pd ( tmp_fs , _mm_set1_pd ( qddoti_data [ ntilecols ] ) ) , tmp_ck )
) ; } } tempR [ 0 ] = 0.0 ; tempR [ 3 ] = - vB -> data [ 6 * b_jcol + 2 ] ;
tempR [ 6 ] = vB -> data [ 6 * b_jcol + 1 ] ; tempR [ 1 ] = vB -> data [ 6 *
b_jcol + 2 ] ; tempR [ 4 ] = 0.0 ; tempR [ 7 ] = - vB -> data [ 6 * b_jcol ]
; tempR [ 2 ] = - vB -> data [ 6 * b_jcol + 1 ] ; tempR [ 5 ] = vB -> data [
6 * b_jcol ] ; tempR [ 8 ] = 0.0 ; tempR_p [ 3 ] = 0.0 ; tempR_p [ 9 ] = - vB
-> data [ 6 * b_jcol + 5 ] ; tempR_p [ 15 ] = vB -> data [ 6 * b_jcol + 4 ] ;
tempR_p [ 4 ] = vB -> data [ 6 * b_jcol + 5 ] ; tempR_p [ 10 ] = 0.0 ;
tempR_p [ 16 ] = - vB -> data [ 6 * b_jcol + 3 ] ; tempR_p [ 5 ] = - vB ->
data [ 6 * b_jcol + 4 ] ; tempR_p [ 11 ] = vB -> data [ 6 * b_jcol + 3 ] ;
tempR_p [ 17 ] = 0.0 ; for ( inner = 0 ; inner < 3 ; inner ++ ) { b_idx_0 =
tempR [ 3 * inner ] ; tempR_p [ 6 * inner ] = b_idx_0 ; tempR_p [ 6 * ( inner
+ 3 ) ] = 0.0 ; tempR_p [ 6 * ( inner + 3 ) + 3 ] = b_idx_0 ; b_idx_0 = tempR
[ 3 * inner + 1 ] ; tempR_p [ 6 * inner + 1 ] = b_idx_0 ; tempR_p [ 6 * (
inner + 3 ) + 1 ] = 0.0 ; tempR_p [ 6 * ( inner + 3 ) + 4 ] = b_idx_0 ;
b_idx_0 = tempR [ 3 * inner + 2 ] ; tempR_p [ 6 * inner + 2 ] = b_idx_0 ;
tempR_p [ 6 * ( inner + 3 ) + 2 ] = 0.0 ; tempR_p [ 6 * ( inner + 3 ) + 5 ] =
b_idx_0 ; } for ( inner = 0 ; inner < 6 ; inner ++ ) { b_idx_0 = 0.0 ; vJ_p [
inner ] = 0.0 ; for ( ntilecols = 0 ; ntilecols < 6 ; ntilecols ++ ) {
b_idx_0 += aB -> data [ ( ( int32_T ) kmjofsvir4 - 1 ) * 6 + ntilecols ] *
X_p -> data [ b_jcol ] . f1 [ 6 * ntilecols + inner ] ; vJ_p [ inner ] +=
tempR_p [ 6 * ntilecols + inner ] * vJ -> data [ 6 * b_jcol + ntilecols ] ; }
qddoti_data [ inner ] = b_idx_0 + locqfeuxtm_data [ inner ] ; } for ( inner =
0 ; inner <= 4 ; inner += 2 ) { tmp_fs = _mm_loadu_pd ( & qddoti_data [ inner
] ) ; tmp_ck = _mm_loadu_pd ( & vJ_p [ inner ] ) ; _mm_storeu_pd ( & aB ->
data [ inner + 6 * b_jcol ] , _mm_add_pd ( tmp_fs , tmp_ck ) ) ; } R [ 0 ] =
0.0 ; R [ 3 ] = - T1 [ 14 ] ; R [ 6 ] = T1 [ 13 ] ; R [ 1 ] = T1 [ 14 ] ; R [
4 ] = 0.0 ; R [ 7 ] = - T1 [ 12 ] ; R [ 2 ] = - T1 [ 13 ] ; R [ 5 ] = T1 [ 12
] ; R [ 8 ] = 0.0 ; for ( inner = 0 ; inner < 3 ; inner ++ ) { for (
ntilecols = 0 ; ntilecols < 3 ; ntilecols ++ ) { tempR [ inner + 3 *
ntilecols ] = 0.0 ; tempR [ inner + 3 * ntilecols ] += T1 [ ntilecols << 2 ]
* R [ inner ] ; tempR [ inner + 3 * ntilecols ] += T1 [ ( ntilecols << 2 ) +
1 ] * R [ inner + 3 ] ; tempR [ inner + 3 * ntilecols ] += T1 [ ( ntilecols
<< 2 ) + 2 ] * R [ inner + 6 ] ; XDHOffset [ ntilecols + 6 * inner ] = T1 [ (
inner << 2 ) + ntilecols ] ; XDHOffset [ ntilecols + 6 * ( inner + 3 ) ] =
0.0 ; } } for ( inner = 0 ; inner < 3 ; inner ++ ) { XDHOffset [ 6 * inner +
3 ] = tempR [ 3 * inner ] ; XDHOffset [ 6 * ( inner + 3 ) + 3 ] = T1 [ inner
<< 2 ] ; XDHOffset [ 6 * inner + 4 ] = tempR [ 3 * inner + 1 ] ; XDHOffset [
6 * ( inner + 3 ) + 4 ] = T1 [ ( inner << 2 ) + 1 ] ; XDHOffset [ 6 * inner +
5 ] = tempR [ 3 * inner + 2 ] ; XDHOffset [ 6 * ( inner + 3 ) + 5 ] = T1 [ (
inner << 2 ) + 2 ] ; } for ( inner = 0 ; inner < 6 ; inner ++ ) { for (
ntilecols = 0 ; ntilecols < 6 ; ntilecols ++ ) { tempR_p [ inner + 6 *
ntilecols ] = 0.0 ; for ( ibcol = 0 ; ibcol < 6 ; ibcol ++ ) { tempR_p [
inner + 6 * ntilecols ] += Xtree -> data [ ( int32_T ) kmjofsvir4 - 1 ] . f1
[ 6 * ibcol + inner ] * XDHOffset [ 6 * ntilecols + ibcol ] ; } } } memcpy (
& Xtree -> data [ b_jcol ] . f1 [ 0 ] , & tempR_p [ 0 ] , 36U * sizeof (
real_T ) ) ; } else { l5ks21uytp ( XDHOffset , S , y ) ; inner = y -> size [
1 ] - 1 ; for ( ntilecols = 0 ; ntilecols < 6 ; ntilecols ++ ) { vB -> data [
ntilecols + 6 * b_jcol ] = vJ -> data [ 6 * b_jcol + ntilecols ] ;
locqfeuxtm_data [ ntilecols ] = 0.0 ; } for ( ntilecols = 0 ; ntilecols <=
inner ; ntilecols ++ ) { aoffset = ntilecols * 6 - 1 ; for ( ibcol = 0 ;
ibcol <= 4 ; ibcol += 2 ) { tmp_fs = _mm_loadu_pd ( & y -> data [ ( aoffset +
ibcol ) + 1 ] ) ; tmp_ck = _mm_loadu_pd ( & locqfeuxtm_data [ ibcol ] ) ;
_mm_storeu_pd ( & locqfeuxtm_data [ ibcol ] , _mm_add_pd ( _mm_mul_pd (
tmp_fs , _mm_set1_pd ( qddoti_data [ ntilecols ] ) ) , tmp_ck ) ) ; } } for (
inner = 0 ; inner < 6 ; inner ++ ) { b_idx_0 = 0.0 ; for ( ntilecols = 0 ;
ntilecols < 6 ; ntilecols ++ ) { b_idx_0 += X_p -> data [ b_jcol ] . f1 [ 6 *
ntilecols + inner ] * a0 [ ntilecols ] ; } aB -> data [ inner + 6 * b_jcol ]
= b_idx_0 + locqfeuxtm_data [ inner ] ; } R [ 0 ] = 0.0 ; R [ 3 ] = - T1 [ 14
] ; R [ 6 ] = T1 [ 13 ] ; R [ 1 ] = T1 [ 14 ] ; R [ 4 ] = 0.0 ; R [ 7 ] = -
T1 [ 12 ] ; R [ 2 ] = - T1 [ 13 ] ; R [ 5 ] = T1 [ 12 ] ; R [ 8 ] = 0.0 ; for
( inner = 0 ; inner < 3 ; inner ++ ) { for ( ntilecols = 0 ; ntilecols < 3 ;
ntilecols ++ ) { tempR [ inner + 3 * ntilecols ] = 0.0 ; tempR [ inner + 3 *
ntilecols ] += T1 [ ntilecols << 2 ] * R [ inner ] ; tempR [ inner + 3 *
ntilecols ] += T1 [ ( ntilecols << 2 ) + 1 ] * R [ inner + 3 ] ; tempR [
inner + 3 * ntilecols ] += T1 [ ( ntilecols << 2 ) + 2 ] * R [ inner + 6 ] ;
Xtree -> data [ b_jcol ] . f1 [ ntilecols + 6 * inner ] = T1 [ ( inner << 2 )
+ ntilecols ] ; Xtree -> data [ b_jcol ] . f1 [ ntilecols + 6 * ( inner + 3 )
] = 0.0 ; } } for ( inner = 0 ; inner < 3 ; inner ++ ) { Xtree -> data [
b_jcol ] . f1 [ 6 * inner + 3 ] = tempR [ 3 * inner ] ; Xtree -> data [
b_jcol ] . f1 [ 6 * ( inner + 3 ) + 3 ] = T1 [ inner << 2 ] ; Xtree -> data [
b_jcol ] . f1 [ 6 * inner + 4 ] = tempR [ 3 * inner + 1 ] ; Xtree -> data [
b_jcol ] . f1 [ 6 * ( inner + 3 ) + 4 ] = T1 [ ( inner << 2 ) + 1 ] ; Xtree
-> data [ b_jcol ] . f1 [ 6 * inner + 5 ] = tempR [ 3 * inner + 2 ] ; Xtree
-> data [ b_jcol ] . f1 [ 6 * ( inner + 3 ) + 5 ] = T1 [ ( inner << 2 ) + 2 ]
; } } for ( inner = 0 ; inner < 36 ; inner ++ ) { XDHOffset [ inner ] = robot
-> Bodies [ b_jcol ] -> SpatialInertia [ inner ] ; } tempR [ 0 ] = 0.0 ;
tempR [ 3 ] = - vB -> data [ 6 * b_jcol + 2 ] ; tempR [ 6 ] = vB -> data [ 6
* b_jcol + 1 ] ; tempR [ 1 ] = vB -> data [ 6 * b_jcol + 2 ] ; tempR [ 4 ] =
0.0 ; tempR [ 7 ] = - vB -> data [ 6 * b_jcol ] ; tempR [ 2 ] = - vB -> data
[ 6 * b_jcol + 1 ] ; tempR [ 5 ] = vB -> data [ 6 * b_jcol ] ; tempR [ 8 ] =
0.0 ; tempR_p [ 18 ] = 0.0 ; tempR_p [ 24 ] = - vB -> data [ 6 * b_jcol + 5 ]
; tempR_p [ 30 ] = vB -> data [ 6 * b_jcol + 4 ] ; tempR_p [ 19 ] = vB ->
data [ 6 * b_jcol + 5 ] ; tempR_p [ 25 ] = 0.0 ; tempR_p [ 31 ] = - vB ->
data [ 6 * b_jcol + 3 ] ; tempR_p [ 20 ] = - vB -> data [ 6 * b_jcol + 4 ] ;
tempR_p [ 26 ] = vB -> data [ 6 * b_jcol + 3 ] ; tempR_p [ 32 ] = 0.0 ; for (
inner = 0 ; inner < 3 ; inner ++ ) { b_idx_0 = tempR [ 3 * inner ] ; tempR_p
[ 6 * inner ] = b_idx_0 ; tempR_p [ 6 * inner + 3 ] = 0.0 ; tempR_p [ 6 * (
inner + 3 ) + 3 ] = b_idx_0 ; b_idx_0 = tempR [ 3 * inner + 1 ] ; tempR_p [ 6
* inner + 1 ] = b_idx_0 ; tempR_p [ 6 * inner + 4 ] = 0.0 ; tempR_p [ 6 * (
inner + 3 ) + 4 ] = b_idx_0 ; b_idx_0 = tempR [ 3 * inner + 2 ] ; tempR_p [ 6
* inner + 2 ] = b_idx_0 ; tempR_p [ 6 * inner + 5 ] = 0.0 ; tempR_p [ 6 * (
inner + 3 ) + 5 ] = b_idx_0 ; } for ( inner = 0 ; inner < 6 ; inner ++ ) {
locqfeuxtm_data [ inner ] = 0.0 ; qddoti_data [ inner ] = 0.0 ; for (
ntilecols = 0 ; ntilecols < 6 ; ntilecols ++ ) { b_idx_0 = XDHOffset [ 6 *
ntilecols + inner ] ; locqfeuxtm_data [ inner ] += vB -> data [ 6 * b_jcol +
ntilecols ] * b_idx_0 ; qddoti_data [ inner ] += aB -> data [ 6 * b_jcol +
ntilecols ] * b_idx_0 ; } } for ( inner = 0 ; inner < 6 ; inner ++ ) { vJ_p [
inner ] = 0.0 ; b_idx_0 = 0.0 ; for ( ntilecols = 0 ; ntilecols < 6 ;
ntilecols ++ ) { b_idx_0 += Xtree -> data [ b_jcol ] . f1 [ 6 * inner +
ntilecols ] * rtP . ExternalForce_Value [ 6 * b_jcol + ntilecols ] ; vJ_p [
inner ] += tempR_p [ 6 * ntilecols + inner ] * locqfeuxtm_data [ ntilecols ]
; } f -> data [ inner + 6 * b_jcol ] = ( qddoti_data [ inner ] + vJ_p [ inner
] ) - b_idx_0 ; } } bdhoypqe5x ( & y ) ; bdhoypqe5x ( & aB ) ; bdhoypqe5x ( &
vB ) ; oxlwy3eg3q ( & Xtree ) ; p = ( int32_T ) - ( ( - 1.0 - n1tv2iovp0 ) +
1.0 ) - 1 ; if ( ( int32_T ) - ( ( - 1.0 - n1tv2iovp0 ) + 1.0 ) - 1 >= 0 ) {
for ( inner = 0 ; inner < 5 ; inner ++ ) { b_p [ inner ] = tmp_kt [ inner ] ;
} } for ( iacol = 0 ; iacol <= p ; iacol ++ ) { lwk1iknpkj = n1tv2iovp0 - (
real_T ) iacol ; obj_i = robot -> Bodies [ ( int32_T ) lwk1iknpkj - 1 ] ;
inner = a -> size [ 0 ] * a -> size [ 1 ] ; a -> size [ 0 ] = 1 ; a -> size [
1 ] = obj_i -> JointInternal . Type -> size [ 1 ] ; e3n3mnwk4b ( a , inner )
; loop_ub = obj_i -> JointInternal . Type -> size [ 1 ] ; for ( inner = 0 ;
inner < loop_ub ; inner ++ ) { a -> data [ inner ] = obj_i -> JointInternal .
Type -> data [ inner ] ; } b_bool = false ; if ( a -> size [ 1 ] != 5 ) { }
else { ntilecols = 1 ; do { exitg1 = 0 ; if ( ntilecols - 1 < 5 ) { if ( a ->
data [ ntilecols - 1 ] != b_p [ ntilecols - 1 ] ) { exitg1 = 1 ; } else {
ntilecols ++ ; } } else { b_bool = true ; exitg1 = 1 ; } } while ( exitg1 ==
0 ) ; } if ( ! b_bool ) { obj_i = robot -> Bodies [ ( int32_T ) lwk1iknpkj -
1 ] ; for ( inner = 0 ; inner < 16 ; inner ++ ) { out [ inner ] = obj_i ->
JointInternal . ChildToJointTransform [ inner ] ; } for ( inner = 0 ; inner <
3 ; inner ++ ) { R [ 3 * inner ] = out [ inner ] ; R [ 3 * inner + 1 ] = out
[ inner + 4 ] ; R [ 3 * inner + 2 ] = out [ inner + 8 ] ; } for ( inner = 0 ;
inner < 9 ; inner ++ ) { tempR [ inner ] = - R [ inner ] ; } for ( inner = 0
; inner < 3 ; inner ++ ) { drvc3knqcj [ inner << 2 ] = R [ 3 * inner ] ;
drvc3knqcj [ ( inner << 2 ) + 1 ] = R [ 3 * inner + 1 ] ; drvc3knqcj [ (
inner << 2 ) + 2 ] = R [ 3 * inner + 2 ] ; drvc3knqcj [ inner + 12 ] = (
tempR [ inner + 3 ] * out [ 13 ] + tempR [ inner ] * out [ 12 ] ) + tempR [
inner + 6 ] * out [ 14 ] ; } drvc3knqcj [ 3 ] = 0.0 ; drvc3knqcj [ 7 ] = 0.0
; drvc3knqcj [ 11 ] = 0.0 ; drvc3knqcj [ 15 ] = 1.0 ; obj_i = robot -> Bodies
[ ( int32_T ) lwk1iknpkj - 1 ] ; inner = vJ -> size [ 0 ] * vJ -> size [ 1 ]
; vJ -> size [ 0 ] = 6 ; vJ -> size [ 1 ] = obj_i -> JointInternal .
MotionSubspace -> size [ 1 ] ; miyl1rq4uj ( vJ , inner ) ; loop_ub = 6 *
obj_i -> JointInternal . MotionSubspace -> size [ 1 ] ; for ( inner = 0 ;
inner < loop_ub ; inner ++ ) { vJ -> data [ inner ] = obj_i -> JointInternal
. MotionSubspace -> data [ inner ] ; } tmp_l [ 0 ] = 0.0 ; tmp_l [ 3 ] = -
drvc3knqcj [ 14 ] ; tmp_l [ 6 ] = drvc3knqcj [ 13 ] ; tmp_l [ 1 ] =
drvc3knqcj [ 14 ] ; tmp_l [ 4 ] = 0.0 ; tmp_l [ 7 ] = - drvc3knqcj [ 12 ] ;
tmp_l [ 2 ] = - drvc3knqcj [ 13 ] ; tmp_l [ 5 ] = drvc3knqcj [ 12 ] ; tmp_l [
8 ] = 0.0 ; for ( inner = 0 ; inner < 3 ; inner ++ ) { for ( ntilecols = 0 ;
ntilecols < 3 ; ntilecols ++ ) { R [ inner + 3 * ntilecols ] = 0.0 ; R [
inner + 3 * ntilecols ] += drvc3knqcj [ ntilecols << 2 ] * tmp_l [ inner ] ;
R [ inner + 3 * ntilecols ] += drvc3knqcj [ ( ntilecols << 2 ) + 1 ] * tmp_l
[ inner + 3 ] ; R [ inner + 3 * ntilecols ] += drvc3knqcj [ ( ntilecols << 2
) + 2 ] * tmp_l [ inner + 6 ] ; XDHOffset [ ntilecols + 6 * inner ] =
drvc3knqcj [ ( inner << 2 ) + ntilecols ] ; XDHOffset [ ntilecols + 6 * (
inner + 3 ) ] = 0.0 ; } } for ( inner = 0 ; inner < 3 ; inner ++ ) {
XDHOffset [ 6 * inner + 3 ] = R [ 3 * inner ] ; XDHOffset [ 6 * ( inner + 3 )
+ 3 ] = drvc3knqcj [ inner << 2 ] ; XDHOffset [ 6 * inner + 4 ] = R [ 3 *
inner + 1 ] ; XDHOffset [ 6 * ( inner + 3 ) + 4 ] = drvc3knqcj [ ( inner << 2
) + 1 ] ; XDHOffset [ 6 * inner + 5 ] = R [ 3 * inner + 2 ] ; XDHOffset [ 6 *
( inner + 3 ) + 5 ] = drvc3knqcj [ ( inner << 2 ) + 2 ] ; } l5ks21uytp (
XDHOffset , vJ , S ) ; ibcol = S -> size [ 1 ] - 1 ; inner = b -> size [ 0 ]
; b -> size [ 0 ] = S -> size [ 1 ] ; miyl1rq4uj ( b , inner ) ; for ( b_jcol
= 0 ; b_jcol <= ibcol ; b_jcol ++ ) { aoffset = b_jcol * 6 - 1 ; kmjofsvir4 =
0.0 ; for ( ntilecols = 0 ; ntilecols < 6 ; ntilecols ++ ) { kmjofsvir4 += f
-> data [ ( ( int32_T ) lwk1iknpkj - 1 ) * 6 + ntilecols ] * S -> data [ (
aoffset + ntilecols ) + 1 ] ; } b -> data [ b_jcol ] = kmjofsvir4 ; } b_idx_0
= robot -> VelocityDoFMap [ ( int32_T ) lwk1iknpkj - 1 ] ; jvo4w5mh0q_idx_0 =
robot -> VelocityDoFMap [ ( int32_T ) lwk1iknpkj + 8 ] ; if ( b_idx_0 >
jvo4w5mh0q_idx_0 ) { b_jcol = 0 ; ntilecols = 0 ; } else { b_jcol = ( int32_T
) b_idx_0 - 1 ; ntilecols = ( int32_T ) jvo4w5mh0q_idx_0 ; } ibcol =
ntilecols - b_jcol ; for ( inner = 0 ; inner < ibcol ; inner ++ ) { rtB .
k3szni1qeu [ b_jcol + inner ] = b -> data [ inner ] ; } } kmjofsvir4 = robot
-> Bodies [ ( int32_T ) lwk1iknpkj - 1 ] -> ParentIndex ; if ( kmjofsvir4 >
0.0 ) { for ( inner = 0 ; inner < 6 ; inner ++ ) { b_idx_0 = 0.0 ; for (
ntilecols = 0 ; ntilecols < 6 ; ntilecols ++ ) { b_idx_0 += f -> data [ ( (
int32_T ) lwk1iknpkj - 1 ) * 6 + ntilecols ] * X_p -> data [ ( int32_T )
lwk1iknpkj - 1 ] . f1 [ 6 * inner + ntilecols ] ; } avi5g2zns4 [ inner ] = f
-> data [ ( ( int32_T ) kmjofsvir4 - 1 ) * 6 + inner ] + b_idx_0 ; } for (
inner = 0 ; inner < 6 ; inner ++ ) { f -> data [ inner + 6 * ( ( int32_T )
kmjofsvir4 - 1 ) ] = avi5g2zns4 [ inner ] ; } } } bdhoypqe5x ( & S ) ;
bdhoypqe5x ( & f ) ; bdhoypqe5x ( & vJ ) ; oxlwy3eg3q ( & X_p ) ; if ( t >
rtP . gripUpperLimit ) { n1tv2iovp0 = t - rtP . gripUpperLimit ; } else if (
t >= rtP . gripLowerLimit ) { n1tv2iovp0 = 0.0 ; } else { n1tv2iovp0 = t -
rtP . gripLowerLimit ; } lwk1iknpkj = rtP . KiGrip * m ; if ( n1tv2iovp0 >
rtP . Clamping_zero_Value ) { tmp_d = rtP . Constant_Value ; } else { tmp_d =
rtP . Constant2_Value ; } if ( lwk1iknpkj > rtP . Clamping_zero_Value ) {
tmp_o = rtP . Constant3_Value_cugk0roo4b ; } else { tmp_o = rtP .
Constant4_Value ; } if ( ( rtP . Clamping_zero_Value != n1tv2iovp0 ) && (
tmp_d == tmp_o ) ) { rtB . h05kbfk1ll = rtP . Constant1_Value ; } else { rtB
. h05kbfk1ll = lwk1iknpkj ; } if ( n > rtP . gripUpperLimit ) { n1tv2iovp0 =
n - rtP . gripUpperLimit ; } else if ( n >= rtP . gripLowerLimit ) {
n1tv2iovp0 = 0.0 ; } else { n1tv2iovp0 = n - rtP . gripLowerLimit ; }
lwk1iknpkj = rtP . KiGrip * k ; if ( n1tv2iovp0 > rtP .
Clamping_zero_Value_hoqagzqhh4 ) { tmp_d = rtP . Constant_Value_bpv2m0vvte ;
} else { tmp_d = rtP . Constant2_Value_potg3ffb2h ; } if ( lwk1iknpkj > rtP .
Clamping_zero_Value_hoqagzqhh4 ) { tmp_o = rtP . Constant3_Value_lqo22zxgur ;
} else { tmp_o = rtP . Constant4_Value_m2oknxe15n ; } if ( ( rtP .
Clamping_zero_Value_hoqagzqhh4 != n1tv2iovp0 ) && ( tmp_d == tmp_o ) ) { rtB
. aualjlkhue = rtP . Constant1_Value_assvjoqkhx ; } else { rtB . aualjlkhue =
lwk1iknpkj ; } if ( ed5ohpi45w_idx_4 > rtP . gripUpperLimit ) { rtB .
p4begt1sri [ 0 ] = rtP . gripUpperLimit ; } else if ( ed5ohpi45w_idx_4 < rtP
. gripLowerLimit ) { rtB . p4begt1sri [ 0 ] = rtP . gripLowerLimit ; } else {
rtB . p4begt1sri [ 0 ] = ed5ohpi45w_idx_4 ; } if ( ed5ohpi45w_idx_5 > rtP .
gripUpperLimit ) { rtB . p4begt1sri [ 1 ] = rtP . gripUpperLimit ; } else if
( ed5ohpi45w_idx_5 < rtP . gripLowerLimit ) { rtB . p4begt1sri [ 1 ] = rtP .
gripLowerLimit ; } else { rtB . p4begt1sri [ 1 ] = ed5ohpi45w_idx_5 ; } if (
ed5ohpi45w_idx_0 > rtP . ctrlUpperLimit ) { rtB . psgggvhrky [ 0 ] = rtP .
ctrlUpperLimit ; } else if ( ed5ohpi45w_idx_0 < rtP . ctrlLowerLimit ) { rtB
. psgggvhrky [ 0 ] = rtP . ctrlLowerLimit ; } else { rtB . psgggvhrky [ 0 ] =
ed5ohpi45w_idx_0 ; } if ( ed5ohpi45w_idx_1 > rtP . ctrlUpperLimit ) { rtB .
psgggvhrky [ 1 ] = rtP . ctrlUpperLimit ; } else if ( ed5ohpi45w_idx_1 < rtP
. ctrlLowerLimit ) { rtB . psgggvhrky [ 1 ] = rtP . ctrlLowerLimit ; } else {
rtB . psgggvhrky [ 1 ] = ed5ohpi45w_idx_1 ; } if ( ed5ohpi45w_idx_2 > rtP .
ctrlUpperLimit ) { rtB . psgggvhrky [ 2 ] = rtP . ctrlUpperLimit ; } else if
( ed5ohpi45w_idx_2 < rtP . ctrlLowerLimit ) { rtB . psgggvhrky [ 2 ] = rtP .
ctrlLowerLimit ; } else { rtB . psgggvhrky [ 2 ] = ed5ohpi45w_idx_2 ; } if (
enjq30wyaa > rtP . ctrlUpperLimit ) { rtB . psgggvhrky [ 3 ] = rtP .
ctrlUpperLimit ; } else if ( enjq30wyaa < rtP . ctrlLowerLimit ) { rtB .
psgggvhrky [ 3 ] = rtP . ctrlLowerLimit ; } else { rtB . psgggvhrky [ 3 ] =
enjq30wyaa ; } } fagcrr3avn ( & a ) ; bdhoypqe5x ( & b ) ; if ( ssIsSampleHit
( rtS , 1 , 0 ) ) { if ( ssIsSampleHit ( rtS , 2 , 0 ) ) { for ( iacol = 0 ;
iacol < 6 ; iacol ++ ) { rtB . nd1rapbf2u [ iacol ] = rtDW . ap5zbvosy0 [
iacol ] ; } } { if ( rtDW . hbahckh543 . AQHandles && ssGetLogOutput ( rtS )
) { sdiWriteSignal ( rtDW . hbahckh543 . AQHandles , ssGetTaskTime ( rtS , 1
) , ( char * ) & rtB . ev24cemjyk [ 3 ] + 0 ) ; } } { if ( rtDW . oesft45bii
. AQHandles && ssGetLogOutput ( rtS ) ) { sdiWriteSignal ( rtDW . oesft45bii
. AQHandles , ssGetTaskTime ( rtS , 1 ) , ( char * ) & rtB . ev24cemjyk [ 4 ]
+ 0 ) ; } } { if ( rtDW . nzulbnsjry . AQHandles && ssGetLogOutput ( rtS ) )
{ sdiWriteSignal ( rtDW . nzulbnsjry . AQHandles , ssGetTaskTime ( rtS , 1 )
, ( char * ) & rtB . ev24cemjyk [ 5 ] + 0 ) ; } } { if ( rtDW . ppxa1k2ehz .
AQHandles && ssGetLogOutput ( rtS ) ) { sdiWriteSignal ( rtDW . ppxa1k2ehz .
AQHandles , ssGetTaskTime ( rtS , 1 ) , ( char * ) & rtB . ev24cemjyk [ 1 ] +
0 ) ; } } { if ( rtDW . axrm20twhx . AQHandles && ssGetLogOutput ( rtS ) ) {
sdiWriteSignal ( rtDW . axrm20twhx . AQHandles , ssGetTaskTime ( rtS , 1 ) ,
( char * ) & rtB . ev24cemjyk [ 0 ] + 0 ) ; } } { if ( rtDW . dygnyfsjhq .
AQHandles && ssGetLogOutput ( rtS ) ) { sdiWriteSignal ( rtDW . dygnyfsjhq .
AQHandles , ssGetTaskTime ( rtS , 1 ) , ( char * ) & rtB . ev24cemjyk [ 2 ] +
0 ) ; } } } rtB . b5i1gadrfg [ 0 ] = rtB . nd1rapbf2u [ 4 ] ; rtB .
b5i1gadrfg [ 1 ] = 0.0 ; rtB . b5i1gadrfg [ 2 ] = 0.0 ; if (
ssIsMajorTimeStep ( rtS ) ) { rtDW . jhwzr3b1pg [ 0 ] = ! ( rtB . b5i1gadrfg
[ 0 ] == rtDW . jhwzr3b1pg [ 1 ] ) ; rtDW . jhwzr3b1pg [ 1 ] = rtB .
b5i1gadrfg [ 0 ] ; } rtB . b5i1gadrfg [ 0 ] = rtDW . jhwzr3b1pg [ 1 ] ; rtB .
b5i1gadrfg [ 3 ] = rtDW . jhwzr3b1pg [ 0 ] ; rtB . o4mrrxpgqr [ 0 ] = rtB .
nd1rapbf2u [ 5 ] ; rtB . o4mrrxpgqr [ 1 ] = 0.0 ; rtB . o4mrrxpgqr [ 2 ] =
0.0 ; if ( ssIsMajorTimeStep ( rtS ) ) { rtDW . eukf1npmbe [ 0 ] = ! ( rtB .
o4mrrxpgqr [ 0 ] == rtDW . eukf1npmbe [ 1 ] ) ; rtDW . eukf1npmbe [ 1 ] = rtB
. o4mrrxpgqr [ 0 ] ; } rtB . o4mrrxpgqr [ 0 ] = rtDW . eukf1npmbe [ 1 ] ; rtB
. o4mrrxpgqr [ 3 ] = rtDW . eukf1npmbe [ 0 ] ; rtB . bpxoa5jqph [ 0 ] = rtB .
nd1rapbf2u [ 0 ] ; rtB . bpxoa5jqph [ 1 ] = 0.0 ; rtB . bpxoa5jqph [ 2 ] =
0.0 ; if ( ssIsMajorTimeStep ( rtS ) ) { rtDW . eutf4o2lqe [ 0 ] = ! ( rtB .
bpxoa5jqph [ 0 ] == rtDW . eutf4o2lqe [ 1 ] ) ; rtDW . eutf4o2lqe [ 1 ] = rtB
. bpxoa5jqph [ 0 ] ; } rtB . bpxoa5jqph [ 0 ] = rtDW . eutf4o2lqe [ 1 ] ; rtB
. bpxoa5jqph [ 3 ] = rtDW . eutf4o2lqe [ 0 ] ; rtB . gtqltersrn [ 0 ] = rtB .
nd1rapbf2u [ 1 ] ; rtB . gtqltersrn [ 1 ] = 0.0 ; rtB . gtqltersrn [ 2 ] =
0.0 ; if ( ssIsMajorTimeStep ( rtS ) ) { rtDW . dwwiwqgqz0 [ 0 ] = ! ( rtB .
gtqltersrn [ 0 ] == rtDW . dwwiwqgqz0 [ 1 ] ) ; rtDW . dwwiwqgqz0 [ 1 ] = rtB
. gtqltersrn [ 0 ] ; } rtB . gtqltersrn [ 0 ] = rtDW . dwwiwqgqz0 [ 1 ] ; rtB
. gtqltersrn [ 3 ] = rtDW . dwwiwqgqz0 [ 0 ] ; rtB . kfmkqerxag [ 0 ] = rtB .
nd1rapbf2u [ 2 ] ; rtB . kfmkqerxag [ 1 ] = 0.0 ; rtB . kfmkqerxag [ 2 ] =
0.0 ; if ( ssIsMajorTimeStep ( rtS ) ) { rtDW . pmpxho4tev [ 0 ] = ! ( rtB .
kfmkqerxag [ 0 ] == rtDW . pmpxho4tev [ 1 ] ) ; rtDW . pmpxho4tev [ 1 ] = rtB
. kfmkqerxag [ 0 ] ; } rtB . kfmkqerxag [ 0 ] = rtDW . pmpxho4tev [ 1 ] ; rtB
. kfmkqerxag [ 3 ] = rtDW . pmpxho4tev [ 0 ] ; rtB . kqkimrfdm5 [ 0 ] = rtB .
nd1rapbf2u [ 3 ] ; rtB . kqkimrfdm5 [ 1 ] = 0.0 ; rtB . kqkimrfdm5 [ 2 ] =
0.0 ; if ( ssIsMajorTimeStep ( rtS ) ) { rtDW . eqedaoza0o [ 0 ] = ! ( rtB .
kqkimrfdm5 [ 0 ] == rtDW . eqedaoza0o [ 1 ] ) ; rtDW . eqedaoza0o [ 1 ] = rtB
. kqkimrfdm5 [ 0 ] ; } rtB . kqkimrfdm5 [ 0 ] = rtDW . eqedaoza0o [ 1 ] ; rtB
. kqkimrfdm5 [ 3 ] = rtDW . eqedaoza0o [ 0 ] ; rtB . grytuooxbk [ 0 ] = rtB .
ev24cemjyk [ 6 ] ; rtB . grytuooxbk [ 1 ] = 0.0 ; rtB . grytuooxbk [ 2 ] =
0.0 ; rtB . grytuooxbk [ 3 ] = 0.0 ; simulationData = ( NeslSimulationData *
) rtDW . khhoybnt1p ; time_m = ssGetT ( rtS ) ; simulationData -> mData ->
mTime . mN = 1 ; simulationData -> mData -> mTime . mX = & time_m ;
simulationData -> mData -> mContStates . mN = 2 ; simulationData -> mData ->
mContStates . mX = & rtX . lnruxliw5a [ 0 ] ; simulationData -> mData ->
mDiscStates . mN = 0 ; simulationData -> mData -> mDiscStates . mX = & rtDW .
awzqwdk31a ; simulationData -> mData -> mModeVector . mN = 0 ; simulationData
-> mData -> mModeVector . mX = & rtDW . onnfopq04a ; b_bool = (
ssIsMajorTimeStep ( rtS ) && ssGetRTWSolverInfo ( rtS ) -> foundContZcEvents
) ; simulationData -> mData -> mFoundZcEvents = b_bool ; simulationData ->
mData -> mIsMajorTimeStep = ssIsMajorTimeStep ( rtS ) ; b_bool = (
ssGetMdlInfoPtr ( rtS ) -> mdlFlags . solverAssertCheck == 1U ) ;
simulationData -> mData -> mIsSolverAssertCheck = b_bool ; b_bool =
ssIsSolverCheckingCIC ( rtS ) ; simulationData -> mData ->
mIsSolverCheckingCIC = b_bool ; b_bool = ssIsSolverComputingJacobian ( rtS )
; simulationData -> mData -> mIsComputingJacobian = b_bool ; simulationData
-> mData -> mIsEvaluatingF0 = ( ssGetEvaluatingF0ForJacobian ( rtS ) != 0 ) ;
b_bool = ssIsSolverRequestingReset ( rtS ) ; simulationData -> mData ->
mIsSolverRequestingReset = b_bool ; simulationData -> mData ->
mIsModeUpdateTimeStep = ssIsModeUpdateTimeStep ( rtS ) ; tmp_g [ 0 ] = 0 ;
tmp_m [ 0 ] = rtB . grytuooxbk [ 0 ] ; tmp_m [ 1 ] = rtB . grytuooxbk [ 1 ] ;
tmp_m [ 2 ] = rtB . grytuooxbk [ 2 ] ; tmp_m [ 3 ] = rtB . grytuooxbk [ 3 ] ;
tmp_g [ 1 ] = 4 ; simulationData -> mData -> mInputValues . mN = 4 ;
simulationData -> mData -> mInputValues . mX = & tmp_m [ 0 ] ; simulationData
-> mData -> mInputOffsets . mN = 2 ; simulationData -> mData -> mInputOffsets
. mX = & tmp_g [ 0 ] ; simulationData -> mData -> mOutputs . mN = 2 ;
simulationData -> mData -> mOutputs . mX = & rtB . i2z2yf5oxo [ 0 ] ;
simulationData -> mData -> mTolerances . mN = 0 ; simulationData -> mData ->
mTolerances . mX = NULL ; simulationData -> mData -> mCstateHasChanged =
false ; time_g = ssGetTaskTime ( rtS , 0 ) ; simulationData -> mData -> mTime
. mN = 1 ; simulationData -> mData -> mTime . mX = & time_g ; simulationData
-> mData -> mSampleHits . mN = 0 ; simulationData -> mData -> mSampleHits .
mX = NULL ; simulationData -> mData -> mIsFundamentalSampleHit = false ;
diagnosticManager = ( NeuDiagnosticManager * ) rtDW . gzao4gihsp ;
diagnosticTree_e = neu_diagnostic_manager_get_initial_tree (
diagnosticManager ) ; ibcol = ne_simulator_method ( ( NeslSimulator * ) rtDW
. dc0bllwdyl , NESL_SIM_OUTPUTS , simulationData , diagnosticManager ) ; if (
ibcol != 0 ) { b_bool = error_buffer_is_empty ( ssGetErrorStatus ( rtS ) ) ;
if ( b_bool ) { msg_e = rtw_diagnostics_msg ( diagnosticTree_e ) ;
ssSetErrorStatus ( rtS , msg_e ) ; } } if ( ssIsMajorTimeStep ( rtS ) &&
simulationData -> mData -> mCstateHasChanged ) {
ssSetBlockStateForSolverChangedAtMajorStep ( rtS ) ; } simulationData = (
NeslSimulationData * ) rtDW . m2vrnf5y3k ; time_j = ssGetT ( rtS ) ;
simulationData -> mData -> mTime . mN = 1 ; simulationData -> mData -> mTime
. mX = & time_j ; simulationData -> mData -> mContStates . mN = 0 ;
simulationData -> mData -> mContStates . mX = NULL ; simulationData -> mData
-> mDiscStates . mN = 0 ; simulationData -> mData -> mDiscStates . mX = &
rtDW . f0i5qqud1u ; simulationData -> mData -> mModeVector . mN = 0 ;
simulationData -> mData -> mModeVector . mX = & rtDW . mzl33nqclq ; b_bool =
( ssIsMajorTimeStep ( rtS ) && ssGetRTWSolverInfo ( rtS ) ->
foundContZcEvents ) ; simulationData -> mData -> mFoundZcEvents = b_bool ;
simulationData -> mData -> mIsMajorTimeStep = ssIsMajorTimeStep ( rtS ) ;
b_bool = ( ssGetMdlInfoPtr ( rtS ) -> mdlFlags . solverAssertCheck == 1U ) ;
simulationData -> mData -> mIsSolverAssertCheck = b_bool ; b_bool =
ssIsSolverCheckingCIC ( rtS ) ; simulationData -> mData ->
mIsSolverCheckingCIC = b_bool ; simulationData -> mData ->
mIsComputingJacobian = false ; simulationData -> mData -> mIsEvaluatingF0 =
false ; b_bool = ssIsSolverRequestingReset ( rtS ) ; simulationData -> mData
-> mIsSolverRequestingReset = b_bool ; simulationData -> mData ->
mIsModeUpdateTimeStep = ssIsModeUpdateTimeStep ( rtS ) ; tmp_f [ 0 ] = 0 ;
tmp_j [ 0 ] = rtB . grytuooxbk [ 0 ] ; tmp_j [ 1 ] = rtB . grytuooxbk [ 1 ] ;
tmp_j [ 2 ] = rtB . grytuooxbk [ 2 ] ; tmp_j [ 3 ] = rtB . grytuooxbk [ 3 ] ;
tmp_f [ 1 ] = 4 ; tmp_j [ 4 ] = rtB . i2z2yf5oxo [ 0 ] ; tmp_j [ 5 ] = rtB .
i2z2yf5oxo [ 1 ] ; tmp_f [ 2 ] = 6 ; simulationData -> mData -> mInputValues
. mN = 6 ; simulationData -> mData -> mInputValues . mX = & tmp_j [ 0 ] ;
simulationData -> mData -> mInputOffsets . mN = 3 ; simulationData -> mData
-> mInputOffsets . mX = & tmp_f [ 0 ] ; simulationData -> mData -> mOutputs .
mN = 2 ; simulationData -> mData -> mOutputs . mX = & rtB . f2ca0jlz0q [ 0 ]
; simulationData -> mData -> mTolerances . mN = 0 ; simulationData -> mData
-> mTolerances . mX = NULL ; simulationData -> mData -> mCstateHasChanged =
false ; time_f = ssGetTaskTime ( rtS , 0 ) ; simulationData -> mData -> mTime
. mN = 1 ; simulationData -> mData -> mTime . mX = & time_f ; simulationData
-> mData -> mSampleHits . mN = 0 ; simulationData -> mData -> mSampleHits .
mX = NULL ; simulationData -> mData -> mIsFundamentalSampleHit = false ;
diagnosticManager = ( NeuDiagnosticManager * ) rtDW . mawdbjpiix ;
diagnosticTree_i = neu_diagnostic_manager_get_initial_tree (
diagnosticManager ) ; ibcol = ne_simulator_method ( ( NeslSimulator * ) rtDW
. ezin4amxyy , NESL_SIM_OUTPUTS , simulationData , diagnosticManager ) ; if (
ibcol != 0 ) { b_bool = error_buffer_is_empty ( ssGetErrorStatus ( rtS ) ) ;
if ( b_bool ) { msg_i = rtw_diagnostics_msg ( diagnosticTree_i ) ;
ssSetErrorStatus ( rtS , msg_i ) ; } } if ( ssIsMajorTimeStep ( rtS ) &&
simulationData -> mData -> mCstateHasChanged ) {
ssSetBlockStateForSolverChangedAtMajorStep ( rtS ) ; } rtB . ffuvz5uef3 [ 0 ]
= rtB . f2ca0jlz0q [ 1 ] ; rtB . ffuvz5uef3 [ 1 ] = 0.0 ; rtB . ffuvz5uef3 [
2 ] = 0.0 ; rtB . ffuvz5uef3 [ 3 ] = 0.0 ; rtB . j5luahqacu [ 0 ] = rtB .
f2ca0jlz0q [ 0 ] ; rtB . j5luahqacu [ 1 ] = 0.0 ; rtB . j5luahqacu [ 2 ] =
0.0 ; rtB . j5luahqacu [ 3 ] = 0.0 ; rtB . n4hlwmps2s [ 0 ] = rtB .
ev24cemjyk [ 7 ] ; rtB . n4hlwmps2s [ 1 ] = 0.0 ; rtB . n4hlwmps2s [ 2 ] =
0.0 ; rtB . n4hlwmps2s [ 3 ] = 0.0 ; simulationData = ( NeslSimulationData *
) rtDW . fg4qcqmocx ; time_c = ssGetT ( rtS ) ; simulationData -> mData ->
mTime . mN = 1 ; simulationData -> mData -> mTime . mX = & time_c ;
simulationData -> mData -> mContStates . mN = 2 ; simulationData -> mData ->
mContStates . mX = & rtX . ipa1byflst [ 0 ] ; simulationData -> mData ->
mDiscStates . mN = 0 ; simulationData -> mData -> mDiscStates . mX = & rtDW .
grpnqhsyz0 ; simulationData -> mData -> mModeVector . mN = 0 ; simulationData
-> mData -> mModeVector . mX = & rtDW . ho0ogmrm4n ; b_bool = (
ssIsMajorTimeStep ( rtS ) && ssGetRTWSolverInfo ( rtS ) -> foundContZcEvents
) ; simulationData -> mData -> mFoundZcEvents = b_bool ; simulationData ->
mData -> mIsMajorTimeStep = ssIsMajorTimeStep ( rtS ) ; b_bool = (
ssGetMdlInfoPtr ( rtS ) -> mdlFlags . solverAssertCheck == 1U ) ;
simulationData -> mData -> mIsSolverAssertCheck = b_bool ; b_bool =
ssIsSolverCheckingCIC ( rtS ) ; simulationData -> mData ->
mIsSolverCheckingCIC = b_bool ; b_bool = ssIsSolverComputingJacobian ( rtS )
; simulationData -> mData -> mIsComputingJacobian = b_bool ; simulationData
-> mData -> mIsEvaluatingF0 = ( ssGetEvaluatingF0ForJacobian ( rtS ) != 0 ) ;
b_bool = ssIsSolverRequestingReset ( rtS ) ; simulationData -> mData ->
mIsSolverRequestingReset = b_bool ; simulationData -> mData ->
mIsModeUpdateTimeStep = ssIsModeUpdateTimeStep ( rtS ) ; tmp_k [ 0 ] = 0 ;
tmp_c [ 0 ] = rtB . n4hlwmps2s [ 0 ] ; tmp_c [ 1 ] = rtB . n4hlwmps2s [ 1 ] ;
tmp_c [ 2 ] = rtB . n4hlwmps2s [ 2 ] ; tmp_c [ 3 ] = rtB . n4hlwmps2s [ 3 ] ;
tmp_k [ 1 ] = 4 ; simulationData -> mData -> mInputValues . mN = 4 ;
simulationData -> mData -> mInputValues . mX = & tmp_c [ 0 ] ; simulationData
-> mData -> mInputOffsets . mN = 2 ; simulationData -> mData -> mInputOffsets
. mX = & tmp_k [ 0 ] ; simulationData -> mData -> mOutputs . mN = 2 ;
simulationData -> mData -> mOutputs . mX = & rtB . ohhfpdrcbe [ 0 ] ;
simulationData -> mData -> mTolerances . mN = 0 ; simulationData -> mData ->
mTolerances . mX = NULL ; simulationData -> mData -> mCstateHasChanged =
false ; time_k = ssGetTaskTime ( rtS , 0 ) ; simulationData -> mData -> mTime
. mN = 1 ; simulationData -> mData -> mTime . mX = & time_k ; simulationData
-> mData -> mSampleHits . mN = 0 ; simulationData -> mData -> mSampleHits .
mX = NULL ; simulationData -> mData -> mIsFundamentalSampleHit = false ;
diagnosticManager = ( NeuDiagnosticManager * ) rtDW . fzld5uaeew ;
diagnosticTree_m = neu_diagnostic_manager_get_initial_tree (
diagnosticManager ) ; ibcol = ne_simulator_method ( ( NeslSimulator * ) rtDW
. nqy5lwwfbt , NESL_SIM_OUTPUTS , simulationData , diagnosticManager ) ; if (
ibcol != 0 ) { b_bool = error_buffer_is_empty ( ssGetErrorStatus ( rtS ) ) ;
if ( b_bool ) { msg_m = rtw_diagnostics_msg ( diagnosticTree_m ) ;
ssSetErrorStatus ( rtS , msg_m ) ; } } if ( ssIsMajorTimeStep ( rtS ) &&
simulationData -> mData -> mCstateHasChanged ) {
ssSetBlockStateForSolverChangedAtMajorStep ( rtS ) ; } simulationData = (
NeslSimulationData * ) rtDW . m3o4jlnlit ; time_b = ssGetT ( rtS ) ;
simulationData -> mData -> mTime . mN = 1 ; simulationData -> mData -> mTime
. mX = & time_b ; simulationData -> mData -> mContStates . mN = 0 ;
simulationData -> mData -> mContStates . mX = NULL ; simulationData -> mData
-> mDiscStates . mN = 0 ; simulationData -> mData -> mDiscStates . mX = &
rtDW . h0rypimiph ; simulationData -> mData -> mModeVector . mN = 0 ;
simulationData -> mData -> mModeVector . mX = & rtDW . k0v0fvu5ab ; b_bool =
( ssIsMajorTimeStep ( rtS ) && ssGetRTWSolverInfo ( rtS ) ->
foundContZcEvents ) ; simulationData -> mData -> mFoundZcEvents = b_bool ;
simulationData -> mData -> mIsMajorTimeStep = ssIsMajorTimeStep ( rtS ) ;
b_bool = ( ssGetMdlInfoPtr ( rtS ) -> mdlFlags . solverAssertCheck == 1U ) ;
simulationData -> mData -> mIsSolverAssertCheck = b_bool ; b_bool =
ssIsSolverCheckingCIC ( rtS ) ; simulationData -> mData ->
mIsSolverCheckingCIC = b_bool ; simulationData -> mData ->
mIsComputingJacobian = false ; simulationData -> mData -> mIsEvaluatingF0 =
false ; b_bool = ssIsSolverRequestingReset ( rtS ) ; simulationData -> mData
-> mIsSolverRequestingReset = b_bool ; simulationData -> mData ->
mIsModeUpdateTimeStep = ssIsModeUpdateTimeStep ( rtS ) ; tmp_n [ 0 ] = 0 ;
tmp_b [ 0 ] = rtB . n4hlwmps2s [ 0 ] ; tmp_b [ 1 ] = rtB . n4hlwmps2s [ 1 ] ;
tmp_b [ 2 ] = rtB . n4hlwmps2s [ 2 ] ; tmp_b [ 3 ] = rtB . n4hlwmps2s [ 3 ] ;
tmp_n [ 1 ] = 4 ; tmp_b [ 4 ] = rtB . ohhfpdrcbe [ 0 ] ; tmp_b [ 5 ] = rtB .
ohhfpdrcbe [ 1 ] ; tmp_n [ 2 ] = 6 ; simulationData -> mData -> mInputValues
. mN = 6 ; simulationData -> mData -> mInputValues . mX = & tmp_b [ 0 ] ;
simulationData -> mData -> mInputOffsets . mN = 3 ; simulationData -> mData
-> mInputOffsets . mX = & tmp_n [ 0 ] ; simulationData -> mData -> mOutputs .
mN = 2 ; simulationData -> mData -> mOutputs . mX = & rtB . mh5qb3h0t1 [ 0 ]
; simulationData -> mData -> mTolerances . mN = 0 ; simulationData -> mData
-> mTolerances . mX = NULL ; simulationData -> mData -> mCstateHasChanged =
false ; time_n = ssGetTaskTime ( rtS , 0 ) ; simulationData -> mData -> mTime
. mN = 1 ; simulationData -> mData -> mTime . mX = & time_n ; simulationData
-> mData -> mSampleHits . mN = 0 ; simulationData -> mData -> mSampleHits .
mX = NULL ; simulationData -> mData -> mIsFundamentalSampleHit = false ;
diagnosticManager = ( NeuDiagnosticManager * ) rtDW . cdslitainn ;
diagnosticTree_g = neu_diagnostic_manager_get_initial_tree (
diagnosticManager ) ; ibcol = ne_simulator_method ( ( NeslSimulator * ) rtDW
. fw4oj40z44 , NESL_SIM_OUTPUTS , simulationData , diagnosticManager ) ; if (
ibcol != 0 ) { b_bool = error_buffer_is_empty ( ssGetErrorStatus ( rtS ) ) ;
if ( b_bool ) { msg_g = rtw_diagnostics_msg ( diagnosticTree_g ) ;
ssSetErrorStatus ( rtS , msg_g ) ; } } if ( ssIsMajorTimeStep ( rtS ) &&
simulationData -> mData -> mCstateHasChanged ) {
ssSetBlockStateForSolverChangedAtMajorStep ( rtS ) ; } rtB . ecaqpvxc3x [ 0 ]
= rtB . mh5qb3h0t1 [ 1 ] ; rtB . ecaqpvxc3x [ 1 ] = 0.0 ; rtB . ecaqpvxc3x [
2 ] = 0.0 ; rtB . ecaqpvxc3x [ 3 ] = 0.0 ; rtB . d5kmywozm5 [ 0 ] = rtB .
mh5qb3h0t1 [ 0 ] ; rtB . d5kmywozm5 [ 1 ] = 0.0 ; rtB . d5kmywozm5 [ 2 ] =
0.0 ; rtB . d5kmywozm5 [ 3 ] = 0.0 ; UNUSED_PARAMETER ( tid ) ; } void
MdlOutputsTID3 ( int_T tid ) { UNUSED_PARAMETER ( tid ) ; } void MdlUpdate (
int_T tid ) { __m128d tmp_c ; __m128d tmp_f ; NeslSimulationData *
simulationData ; NeuDiagnosticManager * diagnosticManager ; NeuDiagnosticTree
* diagnosticTree ; NeuDiagnosticTree * diagnosticTree_e ; NeuDiagnosticTree *
diagnosticTree_p ; char * msg ; char * msg_e ; char * msg_p ; real_T tmp_p [
40 ] ; real_T tmp_g [ 4 ] ; real_T tmp_i [ 4 ] ; real_T time ; real_T time_e
; real_T time_p ; int32_T i ; int_T tmp_e [ 11 ] ; int_T tmp_j [ 2 ] ; int_T
tmp_m [ 2 ] ; boolean_T tmp ; if ( ssIsSampleHit ( rtS , 2 , 0 ) ) { for ( i
= 0 ; i < 6 ; i ++ ) { rtDW . g4g3vrgle4 [ i ] = rtB . hhdb535tvz [ i ] ; } }
simulationData = ( NeslSimulationData * ) rtDW . a5qgeff343 ; time = ssGetT (
rtS ) ; simulationData -> mData -> mTime . mN = 1 ; simulationData -> mData
-> mTime . mX = & time ; simulationData -> mData -> mContStates . mN = 12 ;
simulationData -> mData -> mContStates . mX = & rtX . ge5rtzzo4y [ 0 ] ;
simulationData -> mData -> mDiscStates . mN = 0 ; simulationData -> mData ->
mDiscStates . mX = & rtDW . ofd1yxgqn2 ; simulationData -> mData ->
mModeVector . mN = 0 ; simulationData -> mData -> mModeVector . mX = & rtDW .
edj0t2oelw ; tmp = ( ssIsMajorTimeStep ( rtS ) && ssGetRTWSolverInfo ( rtS )
-> foundContZcEvents ) ; simulationData -> mData -> mFoundZcEvents = tmp ;
simulationData -> mData -> mIsMajorTimeStep = ssIsMajorTimeStep ( rtS ) ; tmp
= ( ssGetMdlInfoPtr ( rtS ) -> mdlFlags . solverAssertCheck == 1U ) ;
simulationData -> mData -> mIsSolverAssertCheck = tmp ; tmp =
ssIsSolverCheckingCIC ( rtS ) ; simulationData -> mData ->
mIsSolverCheckingCIC = tmp ; tmp = ssIsSolverComputingJacobian ( rtS ) ;
simulationData -> mData -> mIsComputingJacobian = tmp ; simulationData ->
mData -> mIsEvaluatingF0 = ( ssGetEvaluatingF0ForJacobian ( rtS ) != 0 ) ;
tmp = ssIsSolverRequestingReset ( rtS ) ; simulationData -> mData ->
mIsSolverRequestingReset = tmp ; simulationData -> mData ->
mIsModeUpdateTimeStep = ssIsModeUpdateTimeStep ( rtS ) ; tmp_e [ 0 ] = 0 ;
tmp_p [ 0 ] = rtB . b5i1gadrfg [ 0 ] ; tmp_p [ 1 ] = rtB . b5i1gadrfg [ 1 ] ;
tmp_p [ 2 ] = rtB . b5i1gadrfg [ 2 ] ; tmp_p [ 3 ] = rtB . b5i1gadrfg [ 3 ] ;
tmp_e [ 1 ] = 4 ; tmp_p [ 4 ] = rtB . o4mrrxpgqr [ 0 ] ; tmp_p [ 5 ] = rtB .
o4mrrxpgqr [ 1 ] ; tmp_p [ 6 ] = rtB . o4mrrxpgqr [ 2 ] ; tmp_p [ 7 ] = rtB .
o4mrrxpgqr [ 3 ] ; tmp_e [ 2 ] = 8 ; tmp_p [ 8 ] = rtB . bpxoa5jqph [ 0 ] ;
tmp_p [ 9 ] = rtB . bpxoa5jqph [ 1 ] ; tmp_p [ 10 ] = rtB . bpxoa5jqph [ 2 ]
; tmp_p [ 11 ] = rtB . bpxoa5jqph [ 3 ] ; tmp_e [ 3 ] = 12 ; tmp_p [ 12 ] =
rtB . gtqltersrn [ 0 ] ; tmp_p [ 13 ] = rtB . gtqltersrn [ 1 ] ; tmp_p [ 14 ]
= rtB . gtqltersrn [ 2 ] ; tmp_p [ 15 ] = rtB . gtqltersrn [ 3 ] ; tmp_e [ 4
] = 16 ; tmp_p [ 16 ] = rtB . kfmkqerxag [ 0 ] ; tmp_p [ 17 ] = rtB .
kfmkqerxag [ 1 ] ; tmp_p [ 18 ] = rtB . kfmkqerxag [ 2 ] ; tmp_p [ 19 ] = rtB
. kfmkqerxag [ 3 ] ; tmp_e [ 5 ] = 20 ; tmp_p [ 20 ] = rtB . kqkimrfdm5 [ 0 ]
; tmp_p [ 21 ] = rtB . kqkimrfdm5 [ 1 ] ; tmp_p [ 22 ] = rtB . kqkimrfdm5 [ 2
] ; tmp_p [ 23 ] = rtB . kqkimrfdm5 [ 3 ] ; tmp_e [ 6 ] = 24 ; tmp_p [ 24 ] =
rtB . ffuvz5uef3 [ 0 ] ; tmp_p [ 25 ] = rtB . ffuvz5uef3 [ 1 ] ; tmp_p [ 26 ]
= rtB . ffuvz5uef3 [ 2 ] ; tmp_p [ 27 ] = rtB . ffuvz5uef3 [ 3 ] ; tmp_e [ 7
] = 28 ; tmp_p [ 28 ] = rtB . j5luahqacu [ 0 ] ; tmp_p [ 29 ] = rtB .
j5luahqacu [ 1 ] ; tmp_p [ 30 ] = rtB . j5luahqacu [ 2 ] ; tmp_p [ 31 ] = rtB
. j5luahqacu [ 3 ] ; tmp_e [ 8 ] = 32 ; tmp_p [ 32 ] = rtB . ecaqpvxc3x [ 0 ]
; tmp_p [ 33 ] = rtB . ecaqpvxc3x [ 1 ] ; tmp_p [ 34 ] = rtB . ecaqpvxc3x [ 2
] ; tmp_p [ 35 ] = rtB . ecaqpvxc3x [ 3 ] ; tmp_e [ 9 ] = 36 ; tmp_p [ 36 ] =
rtB . d5kmywozm5 [ 0 ] ; tmp_p [ 37 ] = rtB . d5kmywozm5 [ 1 ] ; tmp_p [ 38 ]
= rtB . d5kmywozm5 [ 2 ] ; tmp_p [ 39 ] = rtB . d5kmywozm5 [ 3 ] ; tmp_e [ 10
] = 40 ; simulationData -> mData -> mInputValues . mN = 40 ; simulationData
-> mData -> mInputValues . mX = & tmp_p [ 0 ] ; simulationData -> mData ->
mInputOffsets . mN = 11 ; simulationData -> mData -> mInputOffsets . mX = &
tmp_e [ 0 ] ; diagnosticManager = ( NeuDiagnosticManager * ) rtDW .
caiz5lqz5f ; diagnosticTree = neu_diagnostic_manager_get_initial_tree (
diagnosticManager ) ; i = ne_simulator_method ( ( NeslSimulator * ) rtDW .
etcerwyaoj , NESL_SIM_UPDATE , simulationData , diagnosticManager ) ; if ( i
!= 0 ) { tmp = error_buffer_is_empty ( ssGetErrorStatus ( rtS ) ) ; if ( tmp
) { msg = rtw_diagnostics_msg ( diagnosticTree ) ; ssSetErrorStatus ( rtS ,
msg ) ; } } if ( ssIsSampleHit ( rtS , 2 , 0 ) ) { rtDW . ahio0mgx5h [ 0 ] =
( rtB . kj5qpxedtg [ 0 ] - rtDW . ahio0mgx5h [ 0 ] * rtP .
InputFilter_DenCoef [ 1 ] ) / rtP . InputFilter_DenCoef [ 0 ] ; rtDW .
ahio0mgx5h [ 1 ] = ( rtB . kj5qpxedtg [ 1 ] - rtP . InputFilter_DenCoef [ 1 ]
* rtDW . ahio0mgx5h [ 1 ] ) / rtP . InputFilter_DenCoef [ 0 ] ; rtDW .
ahio0mgx5h [ 2 ] = ( rtB . kj5qpxedtg [ 2 ] - rtP . InputFilter_DenCoef [ 1 ]
* rtDW . ahio0mgx5h [ 2 ] ) / rtP . InputFilter_DenCoef [ 0 ] ; rtDW .
ahio0mgx5h [ 3 ] = ( rtB . kj5qpxedtg [ 3 ] - rtP . InputFilter_DenCoef [ 1 ]
* rtDW . ahio0mgx5h [ 3 ] ) / rtP . InputFilter_DenCoef [ 0 ] ; rtDW .
jmlpsyjpqa += rtP . Filter_gainval * rtB . dmuwptdi5m ; rtDW . osgjdwjesl +=
rtP . Filter_gainval_pywx3a4mfz * rtB . crue1g0m5e ; rtDW . fkcgzk5oma += rtP
. Filter_gainval_bis3jwhwo3 * rtB . oolxi5a4g3 ; rtDW . c45aidngho += rtP .
Filter_gainval_l4l4aecji2 * rtB . kl0afniunm ; rtDW . kaentdmtdn += rtP .
Integrator_gainval * rtB . h05kbfk1ll ; rtDW . d5wogx3kqa += rtP .
Integrator_gainval_oyckhxze31 * rtB . aualjlkhue ; for ( i = 0 ; i <= 4 ; i
+= 2 ) { tmp_f = _mm_loadu_pd ( & rtDW . jiqh55j12p [ i ] ) ; tmp_c =
_mm_loadu_pd ( & rtB . k3szni1qeu [ i ] ) ; _mm_storeu_pd ( & rtDW .
jiqh55j12p [ i ] , _mm_div_pd ( _mm_sub_pd ( tmp_c , _mm_mul_pd ( _mm_set1_pd
( rtP . OutputFilter_DenCoef [ 1 ] ) , tmp_f ) ) , _mm_set1_pd ( rtP .
OutputFilter_DenCoef [ 0 ] ) ) ) ; tmp_f = _mm_loadu_pd ( & rtB . nqkvctls4x
[ i ] ) ; _mm_storeu_pd ( & rtDW . ggzlanf4wa [ i ] , tmp_f ) ; tmp_f =
_mm_loadu_pd ( & rtB . gn1fk0rbkj [ i ] ) ; _mm_storeu_pd ( & rtDW .
p4ux5yfv5l [ i ] , tmp_f ) ; } rtDW . ap5zbvosy0 [ 0 ] = rtB . psgggvhrky [ 0
] ; rtDW . ap5zbvosy0 [ 1 ] = rtB . psgggvhrky [ 1 ] ; rtDW . ap5zbvosy0 [ 2
] = rtB . psgggvhrky [ 2 ] ; rtDW . ap5zbvosy0 [ 3 ] = rtB . psgggvhrky [ 3 ]
; rtDW . ap5zbvosy0 [ 4 ] = rtB . p4begt1sri [ 0 ] ; rtDW . ap5zbvosy0 [ 5 ]
= rtB . p4begt1sri [ 1 ] ; } simulationData = ( NeslSimulationData * ) rtDW .
khhoybnt1p ; time_p = ssGetT ( rtS ) ; simulationData -> mData -> mTime . mN
= 1 ; simulationData -> mData -> mTime . mX = & time_p ; simulationData ->
mData -> mContStates . mN = 2 ; simulationData -> mData -> mContStates . mX =
& rtX . lnruxliw5a [ 0 ] ; simulationData -> mData -> mDiscStates . mN = 0 ;
simulationData -> mData -> mDiscStates . mX = & rtDW . awzqwdk31a ;
simulationData -> mData -> mModeVector . mN = 0 ; simulationData -> mData ->
mModeVector . mX = & rtDW . onnfopq04a ; tmp = ( ssIsMajorTimeStep ( rtS ) &&
ssGetRTWSolverInfo ( rtS ) -> foundContZcEvents ) ; simulationData -> mData
-> mFoundZcEvents = tmp ; simulationData -> mData -> mIsMajorTimeStep =
ssIsMajorTimeStep ( rtS ) ; tmp = ( ssGetMdlInfoPtr ( rtS ) -> mdlFlags .
solverAssertCheck == 1U ) ; simulationData -> mData -> mIsSolverAssertCheck =
tmp ; tmp = ssIsSolverCheckingCIC ( rtS ) ; simulationData -> mData ->
mIsSolverCheckingCIC = tmp ; tmp = ssIsSolverComputingJacobian ( rtS ) ;
simulationData -> mData -> mIsComputingJacobian = tmp ; simulationData ->
mData -> mIsEvaluatingF0 = ( ssGetEvaluatingF0ForJacobian ( rtS ) != 0 ) ;
tmp = ssIsSolverRequestingReset ( rtS ) ; simulationData -> mData ->
mIsSolverRequestingReset = tmp ; simulationData -> mData ->
mIsModeUpdateTimeStep = ssIsModeUpdateTimeStep ( rtS ) ; tmp_m [ 0 ] = 0 ;
tmp_i [ 0 ] = rtB . grytuooxbk [ 0 ] ; tmp_i [ 1 ] = rtB . grytuooxbk [ 1 ] ;
tmp_i [ 2 ] = rtB . grytuooxbk [ 2 ] ; tmp_i [ 3 ] = rtB . grytuooxbk [ 3 ] ;
tmp_m [ 1 ] = 4 ; simulationData -> mData -> mInputValues . mN = 4 ;
simulationData -> mData -> mInputValues . mX = & tmp_i [ 0 ] ; simulationData
-> mData -> mInputOffsets . mN = 2 ; simulationData -> mData -> mInputOffsets
. mX = & tmp_m [ 0 ] ; diagnosticManager = ( NeuDiagnosticManager * ) rtDW .
gzao4gihsp ; diagnosticTree_p = neu_diagnostic_manager_get_initial_tree (
diagnosticManager ) ; i = ne_simulator_method ( ( NeslSimulator * ) rtDW .
dc0bllwdyl , NESL_SIM_UPDATE , simulationData , diagnosticManager ) ; if ( i
!= 0 ) { tmp = error_buffer_is_empty ( ssGetErrorStatus ( rtS ) ) ; if ( tmp
) { msg_p = rtw_diagnostics_msg ( diagnosticTree_p ) ; ssSetErrorStatus ( rtS
, msg_p ) ; } } simulationData = ( NeslSimulationData * ) rtDW . fg4qcqmocx ;
time_e = ssGetT ( rtS ) ; simulationData -> mData -> mTime . mN = 1 ;
simulationData -> mData -> mTime . mX = & time_e ; simulationData -> mData ->
mContStates . mN = 2 ; simulationData -> mData -> mContStates . mX = & rtX .
ipa1byflst [ 0 ] ; simulationData -> mData -> mDiscStates . mN = 0 ;
simulationData -> mData -> mDiscStates . mX = & rtDW . grpnqhsyz0 ;
simulationData -> mData -> mModeVector . mN = 0 ; simulationData -> mData ->
mModeVector . mX = & rtDW . ho0ogmrm4n ; tmp = ( ssIsMajorTimeStep ( rtS ) &&
ssGetRTWSolverInfo ( rtS ) -> foundContZcEvents ) ; simulationData -> mData
-> mFoundZcEvents = tmp ; simulationData -> mData -> mIsMajorTimeStep =
ssIsMajorTimeStep ( rtS ) ; tmp = ( ssGetMdlInfoPtr ( rtS ) -> mdlFlags .
solverAssertCheck == 1U ) ; simulationData -> mData -> mIsSolverAssertCheck =
tmp ; tmp = ssIsSolverCheckingCIC ( rtS ) ; simulationData -> mData ->
mIsSolverCheckingCIC = tmp ; tmp = ssIsSolverComputingJacobian ( rtS ) ;
simulationData -> mData -> mIsComputingJacobian = tmp ; simulationData ->
mData -> mIsEvaluatingF0 = ( ssGetEvaluatingF0ForJacobian ( rtS ) != 0 ) ;
tmp = ssIsSolverRequestingReset ( rtS ) ; simulationData -> mData ->
mIsSolverRequestingReset = tmp ; simulationData -> mData ->
mIsModeUpdateTimeStep = ssIsModeUpdateTimeStep ( rtS ) ; tmp_j [ 0 ] = 0 ;
tmp_g [ 0 ] = rtB . n4hlwmps2s [ 0 ] ; tmp_g [ 1 ] = rtB . n4hlwmps2s [ 1 ] ;
tmp_g [ 2 ] = rtB . n4hlwmps2s [ 2 ] ; tmp_g [ 3 ] = rtB . n4hlwmps2s [ 3 ] ;
tmp_j [ 1 ] = 4 ; simulationData -> mData -> mInputValues . mN = 4 ;
simulationData -> mData -> mInputValues . mX = & tmp_g [ 0 ] ; simulationData
-> mData -> mInputOffsets . mN = 2 ; simulationData -> mData -> mInputOffsets
. mX = & tmp_j [ 0 ] ; diagnosticManager = ( NeuDiagnosticManager * ) rtDW .
fzld5uaeew ; diagnosticTree_e = neu_diagnostic_manager_get_initial_tree (
diagnosticManager ) ; i = ne_simulator_method ( ( NeslSimulator * ) rtDW .
nqy5lwwfbt , NESL_SIM_UPDATE , simulationData , diagnosticManager ) ; if ( i
!= 0 ) { tmp = error_buffer_is_empty ( ssGetErrorStatus ( rtS ) ) ; if ( tmp
) { msg_e = rtw_diagnostics_msg ( diagnosticTree_e ) ; ssSetErrorStatus ( rtS
, msg_e ) ; } } UNUSED_PARAMETER ( tid ) ; } void MdlUpdateTID3 ( int_T tid )
{ UNUSED_PARAMETER ( tid ) ; } void MdlDerivatives ( void ) {
NeslSimulationData * simulationData ; NeuDiagnosticManager *
diagnosticManager ; NeuDiagnosticTree * diagnosticTree ; NeuDiagnosticTree *
diagnosticTree_e ; NeuDiagnosticTree * diagnosticTree_p ; XDot * _rtXdot ;
char * msg ; char * msg_e ; char * msg_p ; real_T tmp_p [ 40 ] ; real_T tmp_j
[ 4 ] ; real_T tmp_m [ 4 ] ; real_T time ; real_T time_e ; real_T time_p ;
int32_T tmp_i ; int_T tmp_e [ 11 ] ; int_T tmp_f [ 2 ] ; int_T tmp_g [ 2 ] ;
boolean_T tmp ; _rtXdot = ( ( XDot * ) ssGetdX ( rtS ) ) ; simulationData = (
NeslSimulationData * ) rtDW . a5qgeff343 ; time = ssGetT ( rtS ) ;
simulationData -> mData -> mTime . mN = 1 ; simulationData -> mData -> mTime
. mX = & time ; simulationData -> mData -> mContStates . mN = 12 ;
simulationData -> mData -> mContStates . mX = & rtX . ge5rtzzo4y [ 0 ] ;
simulationData -> mData -> mDiscStates . mN = 0 ; simulationData -> mData ->
mDiscStates . mX = & rtDW . ofd1yxgqn2 ; simulationData -> mData ->
mModeVector . mN = 0 ; simulationData -> mData -> mModeVector . mX = & rtDW .
edj0t2oelw ; tmp = ( ssIsMajorTimeStep ( rtS ) && ssGetRTWSolverInfo ( rtS )
-> foundContZcEvents ) ; simulationData -> mData -> mFoundZcEvents = tmp ;
simulationData -> mData -> mIsMajorTimeStep = ssIsMajorTimeStep ( rtS ) ; tmp
= ( ssGetMdlInfoPtr ( rtS ) -> mdlFlags . solverAssertCheck == 1U ) ;
simulationData -> mData -> mIsSolverAssertCheck = tmp ; tmp =
ssIsSolverCheckingCIC ( rtS ) ; simulationData -> mData ->
mIsSolverCheckingCIC = tmp ; tmp = ssIsSolverComputingJacobian ( rtS ) ;
simulationData -> mData -> mIsComputingJacobian = tmp ; simulationData ->
mData -> mIsEvaluatingF0 = ( ssGetEvaluatingF0ForJacobian ( rtS ) != 0 ) ;
tmp = ssIsSolverRequestingReset ( rtS ) ; simulationData -> mData ->
mIsSolverRequestingReset = tmp ; simulationData -> mData ->
mIsModeUpdateTimeStep = ssIsModeUpdateTimeStep ( rtS ) ; tmp_e [ 0 ] = 0 ;
tmp_p [ 0 ] = rtB . b5i1gadrfg [ 0 ] ; tmp_p [ 1 ] = rtB . b5i1gadrfg [ 1 ] ;
tmp_p [ 2 ] = rtB . b5i1gadrfg [ 2 ] ; tmp_p [ 3 ] = rtB . b5i1gadrfg [ 3 ] ;
tmp_e [ 1 ] = 4 ; tmp_p [ 4 ] = rtB . o4mrrxpgqr [ 0 ] ; tmp_p [ 5 ] = rtB .
o4mrrxpgqr [ 1 ] ; tmp_p [ 6 ] = rtB . o4mrrxpgqr [ 2 ] ; tmp_p [ 7 ] = rtB .
o4mrrxpgqr [ 3 ] ; tmp_e [ 2 ] = 8 ; tmp_p [ 8 ] = rtB . bpxoa5jqph [ 0 ] ;
tmp_p [ 9 ] = rtB . bpxoa5jqph [ 1 ] ; tmp_p [ 10 ] = rtB . bpxoa5jqph [ 2 ]
; tmp_p [ 11 ] = rtB . bpxoa5jqph [ 3 ] ; tmp_e [ 3 ] = 12 ; tmp_p [ 12 ] =
rtB . gtqltersrn [ 0 ] ; tmp_p [ 13 ] = rtB . gtqltersrn [ 1 ] ; tmp_p [ 14 ]
= rtB . gtqltersrn [ 2 ] ; tmp_p [ 15 ] = rtB . gtqltersrn [ 3 ] ; tmp_e [ 4
] = 16 ; tmp_p [ 16 ] = rtB . kfmkqerxag [ 0 ] ; tmp_p [ 17 ] = rtB .
kfmkqerxag [ 1 ] ; tmp_p [ 18 ] = rtB . kfmkqerxag [ 2 ] ; tmp_p [ 19 ] = rtB
. kfmkqerxag [ 3 ] ; tmp_e [ 5 ] = 20 ; tmp_p [ 20 ] = rtB . kqkimrfdm5 [ 0 ]
; tmp_p [ 21 ] = rtB . kqkimrfdm5 [ 1 ] ; tmp_p [ 22 ] = rtB . kqkimrfdm5 [ 2
] ; tmp_p [ 23 ] = rtB . kqkimrfdm5 [ 3 ] ; tmp_e [ 6 ] = 24 ; tmp_p [ 24 ] =
rtB . ffuvz5uef3 [ 0 ] ; tmp_p [ 25 ] = rtB . ffuvz5uef3 [ 1 ] ; tmp_p [ 26 ]
= rtB . ffuvz5uef3 [ 2 ] ; tmp_p [ 27 ] = rtB . ffuvz5uef3 [ 3 ] ; tmp_e [ 7
] = 28 ; tmp_p [ 28 ] = rtB . j5luahqacu [ 0 ] ; tmp_p [ 29 ] = rtB .
j5luahqacu [ 1 ] ; tmp_p [ 30 ] = rtB . j5luahqacu [ 2 ] ; tmp_p [ 31 ] = rtB
. j5luahqacu [ 3 ] ; tmp_e [ 8 ] = 32 ; tmp_p [ 32 ] = rtB . ecaqpvxc3x [ 0 ]
; tmp_p [ 33 ] = rtB . ecaqpvxc3x [ 1 ] ; tmp_p [ 34 ] = rtB . ecaqpvxc3x [ 2
] ; tmp_p [ 35 ] = rtB . ecaqpvxc3x [ 3 ] ; tmp_e [ 9 ] = 36 ; tmp_p [ 36 ] =
rtB . d5kmywozm5 [ 0 ] ; tmp_p [ 37 ] = rtB . d5kmywozm5 [ 1 ] ; tmp_p [ 38 ]
= rtB . d5kmywozm5 [ 2 ] ; tmp_p [ 39 ] = rtB . d5kmywozm5 [ 3 ] ; tmp_e [ 10
] = 40 ; simulationData -> mData -> mInputValues . mN = 40 ; simulationData
-> mData -> mInputValues . mX = & tmp_p [ 0 ] ; simulationData -> mData ->
mInputOffsets . mN = 11 ; simulationData -> mData -> mInputOffsets . mX = &
tmp_e [ 0 ] ; simulationData -> mData -> mDx . mN = 12 ; simulationData ->
mData -> mDx . mX = & _rtXdot -> ge5rtzzo4y [ 0 ] ; diagnosticManager = (
NeuDiagnosticManager * ) rtDW . caiz5lqz5f ; diagnosticTree =
neu_diagnostic_manager_get_initial_tree ( diagnosticManager ) ; tmp_i =
ne_simulator_method ( ( NeslSimulator * ) rtDW . etcerwyaoj ,
NESL_SIM_DERIVATIVES , simulationData , diagnosticManager ) ; if ( tmp_i != 0
) { tmp = error_buffer_is_empty ( ssGetErrorStatus ( rtS ) ) ; if ( tmp ) {
msg = rtw_diagnostics_msg ( diagnosticTree ) ; ssSetErrorStatus ( rtS , msg )
; } } simulationData = ( NeslSimulationData * ) rtDW . khhoybnt1p ; time_p =
ssGetT ( rtS ) ; simulationData -> mData -> mTime . mN = 1 ; simulationData
-> mData -> mTime . mX = & time_p ; simulationData -> mData -> mContStates .
mN = 2 ; simulationData -> mData -> mContStates . mX = & rtX . lnruxliw5a [ 0
] ; simulationData -> mData -> mDiscStates . mN = 0 ; simulationData -> mData
-> mDiscStates . mX = & rtDW . awzqwdk31a ; simulationData -> mData ->
mModeVector . mN = 0 ; simulationData -> mData -> mModeVector . mX = & rtDW .
onnfopq04a ; tmp = ( ssIsMajorTimeStep ( rtS ) && ssGetRTWSolverInfo ( rtS )
-> foundContZcEvents ) ; simulationData -> mData -> mFoundZcEvents = tmp ;
simulationData -> mData -> mIsMajorTimeStep = ssIsMajorTimeStep ( rtS ) ; tmp
= ( ssGetMdlInfoPtr ( rtS ) -> mdlFlags . solverAssertCheck == 1U ) ;
simulationData -> mData -> mIsSolverAssertCheck = tmp ; tmp =
ssIsSolverCheckingCIC ( rtS ) ; simulationData -> mData ->
mIsSolverCheckingCIC = tmp ; tmp = ssIsSolverComputingJacobian ( rtS ) ;
simulationData -> mData -> mIsComputingJacobian = tmp ; simulationData ->
mData -> mIsEvaluatingF0 = ( ssGetEvaluatingF0ForJacobian ( rtS ) != 0 ) ;
tmp = ssIsSolverRequestingReset ( rtS ) ; simulationData -> mData ->
mIsSolverRequestingReset = tmp ; simulationData -> mData ->
mIsModeUpdateTimeStep = ssIsModeUpdateTimeStep ( rtS ) ; tmp_g [ 0 ] = 0 ;
tmp_m [ 0 ] = rtB . grytuooxbk [ 0 ] ; tmp_m [ 1 ] = rtB . grytuooxbk [ 1 ] ;
tmp_m [ 2 ] = rtB . grytuooxbk [ 2 ] ; tmp_m [ 3 ] = rtB . grytuooxbk [ 3 ] ;
tmp_g [ 1 ] = 4 ; simulationData -> mData -> mInputValues . mN = 4 ;
simulationData -> mData -> mInputValues . mX = & tmp_m [ 0 ] ; simulationData
-> mData -> mInputOffsets . mN = 2 ; simulationData -> mData -> mInputOffsets
. mX = & tmp_g [ 0 ] ; simulationData -> mData -> mDx . mN = 2 ;
simulationData -> mData -> mDx . mX = & _rtXdot -> lnruxliw5a [ 0 ] ;
diagnosticManager = ( NeuDiagnosticManager * ) rtDW . gzao4gihsp ;
diagnosticTree_p = neu_diagnostic_manager_get_initial_tree (
diagnosticManager ) ; tmp_i = ne_simulator_method ( ( NeslSimulator * ) rtDW
. dc0bllwdyl , NESL_SIM_DERIVATIVES , simulationData , diagnosticManager ) ;
if ( tmp_i != 0 ) { tmp = error_buffer_is_empty ( ssGetErrorStatus ( rtS ) )
; if ( tmp ) { msg_p = rtw_diagnostics_msg ( diagnosticTree_p ) ;
ssSetErrorStatus ( rtS , msg_p ) ; } } simulationData = ( NeslSimulationData
* ) rtDW . fg4qcqmocx ; time_e = ssGetT ( rtS ) ; simulationData -> mData ->
mTime . mN = 1 ; simulationData -> mData -> mTime . mX = & time_e ;
simulationData -> mData -> mContStates . mN = 2 ; simulationData -> mData ->
mContStates . mX = & rtX . ipa1byflst [ 0 ] ; simulationData -> mData ->
mDiscStates . mN = 0 ; simulationData -> mData -> mDiscStates . mX = & rtDW .
grpnqhsyz0 ; simulationData -> mData -> mModeVector . mN = 0 ; simulationData
-> mData -> mModeVector . mX = & rtDW . ho0ogmrm4n ; tmp = (
ssIsMajorTimeStep ( rtS ) && ssGetRTWSolverInfo ( rtS ) -> foundContZcEvents
) ; simulationData -> mData -> mFoundZcEvents = tmp ; simulationData -> mData
-> mIsMajorTimeStep = ssIsMajorTimeStep ( rtS ) ; tmp = ( ssGetMdlInfoPtr (
rtS ) -> mdlFlags . solverAssertCheck == 1U ) ; simulationData -> mData ->
mIsSolverAssertCheck = tmp ; tmp = ssIsSolverCheckingCIC ( rtS ) ;
simulationData -> mData -> mIsSolverCheckingCIC = tmp ; tmp =
ssIsSolverComputingJacobian ( rtS ) ; simulationData -> mData ->
mIsComputingJacobian = tmp ; simulationData -> mData -> mIsEvaluatingF0 = (
ssGetEvaluatingF0ForJacobian ( rtS ) != 0 ) ; tmp = ssIsSolverRequestingReset
( rtS ) ; simulationData -> mData -> mIsSolverRequestingReset = tmp ;
simulationData -> mData -> mIsModeUpdateTimeStep = ssIsModeUpdateTimeStep (
rtS ) ; tmp_f [ 0 ] = 0 ; tmp_j [ 0 ] = rtB . n4hlwmps2s [ 0 ] ; tmp_j [ 1 ]
= rtB . n4hlwmps2s [ 1 ] ; tmp_j [ 2 ] = rtB . n4hlwmps2s [ 2 ] ; tmp_j [ 3 ]
= rtB . n4hlwmps2s [ 3 ] ; tmp_f [ 1 ] = 4 ; simulationData -> mData ->
mInputValues . mN = 4 ; simulationData -> mData -> mInputValues . mX = &
tmp_j [ 0 ] ; simulationData -> mData -> mInputOffsets . mN = 2 ;
simulationData -> mData -> mInputOffsets . mX = & tmp_f [ 0 ] ;
simulationData -> mData -> mDx . mN = 2 ; simulationData -> mData -> mDx . mX
= & _rtXdot -> ipa1byflst [ 0 ] ; diagnosticManager = ( NeuDiagnosticManager
* ) rtDW . fzld5uaeew ; diagnosticTree_e =
neu_diagnostic_manager_get_initial_tree ( diagnosticManager ) ; tmp_i =
ne_simulator_method ( ( NeslSimulator * ) rtDW . nqy5lwwfbt ,
NESL_SIM_DERIVATIVES , simulationData , diagnosticManager ) ; if ( tmp_i != 0
) { tmp = error_buffer_is_empty ( ssGetErrorStatus ( rtS ) ) ; if ( tmp ) {
msg_e = rtw_diagnostics_msg ( diagnosticTree_e ) ; ssSetErrorStatus ( rtS ,
msg_e ) ; } } } void MdlProjection ( void ) { NeslSimulationData *
simulationData ; NeuDiagnosticManager * diagnosticManager ; NeuDiagnosticTree
* diagnosticTree ; char * msg ; real_T tmp_p [ 40 ] ; real_T time ; int32_T
tmp_i ; int_T tmp_e [ 11 ] ; boolean_T tmp ; simulationData = (
NeslSimulationData * ) rtDW . a5qgeff343 ; time = ssGetT ( rtS ) ;
simulationData -> mData -> mTime . mN = 1 ; simulationData -> mData -> mTime
. mX = & time ; simulationData -> mData -> mContStates . mN = 12 ;
simulationData -> mData -> mContStates . mX = & rtX . ge5rtzzo4y [ 0 ] ;
simulationData -> mData -> mDiscStates . mN = 0 ; simulationData -> mData ->
mDiscStates . mX = & rtDW . ofd1yxgqn2 ; simulationData -> mData ->
mModeVector . mN = 0 ; simulationData -> mData -> mModeVector . mX = & rtDW .
edj0t2oelw ; tmp = ( ssIsMajorTimeStep ( rtS ) && ssGetRTWSolverInfo ( rtS )
-> foundContZcEvents ) ; simulationData -> mData -> mFoundZcEvents = tmp ;
simulationData -> mData -> mIsMajorTimeStep = ssIsMajorTimeStep ( rtS ) ; tmp
= ( ssGetMdlInfoPtr ( rtS ) -> mdlFlags . solverAssertCheck == 1U ) ;
simulationData -> mData -> mIsSolverAssertCheck = tmp ; tmp =
ssIsSolverCheckingCIC ( rtS ) ; simulationData -> mData ->
mIsSolverCheckingCIC = tmp ; tmp = ssIsSolverComputingJacobian ( rtS ) ;
simulationData -> mData -> mIsComputingJacobian = tmp ; simulationData ->
mData -> mIsEvaluatingF0 = ( ssGetEvaluatingF0ForJacobian ( rtS ) != 0 ) ;
tmp = ssIsSolverRequestingReset ( rtS ) ; simulationData -> mData ->
mIsSolverRequestingReset = tmp ; simulationData -> mData ->
mIsModeUpdateTimeStep = ssIsModeUpdateTimeStep ( rtS ) ; tmp_e [ 0 ] = 0 ;
tmp_p [ 0 ] = rtB . b5i1gadrfg [ 0 ] ; tmp_p [ 1 ] = rtB . b5i1gadrfg [ 1 ] ;
tmp_p [ 2 ] = rtB . b5i1gadrfg [ 2 ] ; tmp_p [ 3 ] = rtB . b5i1gadrfg [ 3 ] ;
tmp_e [ 1 ] = 4 ; tmp_p [ 4 ] = rtB . o4mrrxpgqr [ 0 ] ; tmp_p [ 5 ] = rtB .
o4mrrxpgqr [ 1 ] ; tmp_p [ 6 ] = rtB . o4mrrxpgqr [ 2 ] ; tmp_p [ 7 ] = rtB .
o4mrrxpgqr [ 3 ] ; tmp_e [ 2 ] = 8 ; tmp_p [ 8 ] = rtB . bpxoa5jqph [ 0 ] ;
tmp_p [ 9 ] = rtB . bpxoa5jqph [ 1 ] ; tmp_p [ 10 ] = rtB . bpxoa5jqph [ 2 ]
; tmp_p [ 11 ] = rtB . bpxoa5jqph [ 3 ] ; tmp_e [ 3 ] = 12 ; tmp_p [ 12 ] =
rtB . gtqltersrn [ 0 ] ; tmp_p [ 13 ] = rtB . gtqltersrn [ 1 ] ; tmp_p [ 14 ]
= rtB . gtqltersrn [ 2 ] ; tmp_p [ 15 ] = rtB . gtqltersrn [ 3 ] ; tmp_e [ 4
] = 16 ; tmp_p [ 16 ] = rtB . kfmkqerxag [ 0 ] ; tmp_p [ 17 ] = rtB .
kfmkqerxag [ 1 ] ; tmp_p [ 18 ] = rtB . kfmkqerxag [ 2 ] ; tmp_p [ 19 ] = rtB
. kfmkqerxag [ 3 ] ; tmp_e [ 5 ] = 20 ; tmp_p [ 20 ] = rtB . kqkimrfdm5 [ 0 ]
; tmp_p [ 21 ] = rtB . kqkimrfdm5 [ 1 ] ; tmp_p [ 22 ] = rtB . kqkimrfdm5 [ 2
] ; tmp_p [ 23 ] = rtB . kqkimrfdm5 [ 3 ] ; tmp_e [ 6 ] = 24 ; tmp_p [ 24 ] =
rtB . ffuvz5uef3 [ 0 ] ; tmp_p [ 25 ] = rtB . ffuvz5uef3 [ 1 ] ; tmp_p [ 26 ]
= rtB . ffuvz5uef3 [ 2 ] ; tmp_p [ 27 ] = rtB . ffuvz5uef3 [ 3 ] ; tmp_e [ 7
] = 28 ; tmp_p [ 28 ] = rtB . j5luahqacu [ 0 ] ; tmp_p [ 29 ] = rtB .
j5luahqacu [ 1 ] ; tmp_p [ 30 ] = rtB . j5luahqacu [ 2 ] ; tmp_p [ 31 ] = rtB
. j5luahqacu [ 3 ] ; tmp_e [ 8 ] = 32 ; tmp_p [ 32 ] = rtB . ecaqpvxc3x [ 0 ]
; tmp_p [ 33 ] = rtB . ecaqpvxc3x [ 1 ] ; tmp_p [ 34 ] = rtB . ecaqpvxc3x [ 2
] ; tmp_p [ 35 ] = rtB . ecaqpvxc3x [ 3 ] ; tmp_e [ 9 ] = 36 ; tmp_p [ 36 ] =
rtB . d5kmywozm5 [ 0 ] ; tmp_p [ 37 ] = rtB . d5kmywozm5 [ 1 ] ; tmp_p [ 38 ]
= rtB . d5kmywozm5 [ 2 ] ; tmp_p [ 39 ] = rtB . d5kmywozm5 [ 3 ] ; tmp_e [ 10
] = 40 ; simulationData -> mData -> mInputValues . mN = 40 ; simulationData
-> mData -> mInputValues . mX = & tmp_p [ 0 ] ; simulationData -> mData ->
mInputOffsets . mN = 11 ; simulationData -> mData -> mInputOffsets . mX = &
tmp_e [ 0 ] ; diagnosticManager = ( NeuDiagnosticManager * ) rtDW .
caiz5lqz5f ; diagnosticTree = neu_diagnostic_manager_get_initial_tree (
diagnosticManager ) ; tmp_i = ne_simulator_method ( ( NeslSimulator * ) rtDW
. etcerwyaoj , NESL_SIM_PROJECTION , simulationData , diagnosticManager ) ;
if ( tmp_i != 0 ) { tmp = error_buffer_is_empty ( ssGetErrorStatus ( rtS ) )
; if ( tmp ) { msg = rtw_diagnostics_msg ( diagnosticTree ) ;
ssSetErrorStatus ( rtS , msg ) ; } } } void MdlForcingFunction ( void ) {
NeslSimulationData * simulationData ; NeuDiagnosticManager *
diagnosticManager ; NeuDiagnosticTree * diagnosticTree ; NeuDiagnosticTree *
diagnosticTree_e ; NeuDiagnosticTree * diagnosticTree_p ; XDot * _rtXdot ;
char * msg ; char * msg_e ; char * msg_p ; real_T tmp_p [ 40 ] ; real_T tmp_j
[ 4 ] ; real_T tmp_m [ 4 ] ; real_T time ; real_T time_e ; real_T time_p ;
int32_T tmp_i ; int_T tmp_e [ 11 ] ; int_T tmp_f [ 2 ] ; int_T tmp_g [ 2 ] ;
boolean_T tmp ; _rtXdot = ( ( XDot * ) ssGetdX ( rtS ) ) ; simulationData = (
NeslSimulationData * ) rtDW . a5qgeff343 ; time = ssGetT ( rtS ) ;
simulationData -> mData -> mTime . mN = 1 ; simulationData -> mData -> mTime
. mX = & time ; simulationData -> mData -> mContStates . mN = 12 ;
simulationData -> mData -> mContStates . mX = & rtX . ge5rtzzo4y [ 0 ] ;
simulationData -> mData -> mDiscStates . mN = 0 ; simulationData -> mData ->
mDiscStates . mX = & rtDW . ofd1yxgqn2 ; simulationData -> mData ->
mModeVector . mN = 0 ; simulationData -> mData -> mModeVector . mX = & rtDW .
edj0t2oelw ; tmp = ( ssIsMajorTimeStep ( rtS ) && ssGetRTWSolverInfo ( rtS )
-> foundContZcEvents ) ; simulationData -> mData -> mFoundZcEvents = tmp ;
simulationData -> mData -> mIsMajorTimeStep = ssIsMajorTimeStep ( rtS ) ; tmp
= ( ssGetMdlInfoPtr ( rtS ) -> mdlFlags . solverAssertCheck == 1U ) ;
simulationData -> mData -> mIsSolverAssertCheck = tmp ; tmp =
ssIsSolverCheckingCIC ( rtS ) ; simulationData -> mData ->
mIsSolverCheckingCIC = tmp ; tmp = ssIsSolverComputingJacobian ( rtS ) ;
simulationData -> mData -> mIsComputingJacobian = tmp ; simulationData ->
mData -> mIsEvaluatingF0 = ( ssGetEvaluatingF0ForJacobian ( rtS ) != 0 ) ;
tmp = ssIsSolverRequestingReset ( rtS ) ; simulationData -> mData ->
mIsSolverRequestingReset = tmp ; simulationData -> mData ->
mIsModeUpdateTimeStep = ssIsModeUpdateTimeStep ( rtS ) ; tmp_e [ 0 ] = 0 ;
tmp_p [ 0 ] = rtB . b5i1gadrfg [ 0 ] ; tmp_p [ 1 ] = rtB . b5i1gadrfg [ 1 ] ;
tmp_p [ 2 ] = rtB . b5i1gadrfg [ 2 ] ; tmp_p [ 3 ] = rtB . b5i1gadrfg [ 3 ] ;
tmp_e [ 1 ] = 4 ; tmp_p [ 4 ] = rtB . o4mrrxpgqr [ 0 ] ; tmp_p [ 5 ] = rtB .
o4mrrxpgqr [ 1 ] ; tmp_p [ 6 ] = rtB . o4mrrxpgqr [ 2 ] ; tmp_p [ 7 ] = rtB .
o4mrrxpgqr [ 3 ] ; tmp_e [ 2 ] = 8 ; tmp_p [ 8 ] = rtB . bpxoa5jqph [ 0 ] ;
tmp_p [ 9 ] = rtB . bpxoa5jqph [ 1 ] ; tmp_p [ 10 ] = rtB . bpxoa5jqph [ 2 ]
; tmp_p [ 11 ] = rtB . bpxoa5jqph [ 3 ] ; tmp_e [ 3 ] = 12 ; tmp_p [ 12 ] =
rtB . gtqltersrn [ 0 ] ; tmp_p [ 13 ] = rtB . gtqltersrn [ 1 ] ; tmp_p [ 14 ]
= rtB . gtqltersrn [ 2 ] ; tmp_p [ 15 ] = rtB . gtqltersrn [ 3 ] ; tmp_e [ 4
] = 16 ; tmp_p [ 16 ] = rtB . kfmkqerxag [ 0 ] ; tmp_p [ 17 ] = rtB .
kfmkqerxag [ 1 ] ; tmp_p [ 18 ] = rtB . kfmkqerxag [ 2 ] ; tmp_p [ 19 ] = rtB
. kfmkqerxag [ 3 ] ; tmp_e [ 5 ] = 20 ; tmp_p [ 20 ] = rtB . kqkimrfdm5 [ 0 ]
; tmp_p [ 21 ] = rtB . kqkimrfdm5 [ 1 ] ; tmp_p [ 22 ] = rtB . kqkimrfdm5 [ 2
] ; tmp_p [ 23 ] = rtB . kqkimrfdm5 [ 3 ] ; tmp_e [ 6 ] = 24 ; tmp_p [ 24 ] =
rtB . ffuvz5uef3 [ 0 ] ; tmp_p [ 25 ] = rtB . ffuvz5uef3 [ 1 ] ; tmp_p [ 26 ]
= rtB . ffuvz5uef3 [ 2 ] ; tmp_p [ 27 ] = rtB . ffuvz5uef3 [ 3 ] ; tmp_e [ 7
] = 28 ; tmp_p [ 28 ] = rtB . j5luahqacu [ 0 ] ; tmp_p [ 29 ] = rtB .
j5luahqacu [ 1 ] ; tmp_p [ 30 ] = rtB . j5luahqacu [ 2 ] ; tmp_p [ 31 ] = rtB
. j5luahqacu [ 3 ] ; tmp_e [ 8 ] = 32 ; tmp_p [ 32 ] = rtB . ecaqpvxc3x [ 0 ]
; tmp_p [ 33 ] = rtB . ecaqpvxc3x [ 1 ] ; tmp_p [ 34 ] = rtB . ecaqpvxc3x [ 2
] ; tmp_p [ 35 ] = rtB . ecaqpvxc3x [ 3 ] ; tmp_e [ 9 ] = 36 ; tmp_p [ 36 ] =
rtB . d5kmywozm5 [ 0 ] ; tmp_p [ 37 ] = rtB . d5kmywozm5 [ 1 ] ; tmp_p [ 38 ]
= rtB . d5kmywozm5 [ 2 ] ; tmp_p [ 39 ] = rtB . d5kmywozm5 [ 3 ] ; tmp_e [ 10
] = 40 ; simulationData -> mData -> mInputValues . mN = 40 ; simulationData
-> mData -> mInputValues . mX = & tmp_p [ 0 ] ; simulationData -> mData ->
mInputOffsets . mN = 11 ; simulationData -> mData -> mInputOffsets . mX = &
tmp_e [ 0 ] ; simulationData -> mData -> mDx . mN = 12 ; simulationData ->
mData -> mDx . mX = & _rtXdot -> ge5rtzzo4y [ 0 ] ; diagnosticManager = (
NeuDiagnosticManager * ) rtDW . caiz5lqz5f ; diagnosticTree =
neu_diagnostic_manager_get_initial_tree ( diagnosticManager ) ; tmp_i =
ne_simulator_method ( ( NeslSimulator * ) rtDW . etcerwyaoj ,
NESL_SIM_DERIVATIVES , simulationData , diagnosticManager ) ; if ( tmp_i != 0
) { tmp = error_buffer_is_empty ( ssGetErrorStatus ( rtS ) ) ; if ( tmp ) {
msg = rtw_diagnostics_msg ( diagnosticTree ) ; ssSetErrorStatus ( rtS , msg )
; } } simulationData = ( NeslSimulationData * ) rtDW . khhoybnt1p ; time_p =
ssGetT ( rtS ) ; simulationData -> mData -> mTime . mN = 1 ; simulationData
-> mData -> mTime . mX = & time_p ; simulationData -> mData -> mContStates .
mN = 2 ; simulationData -> mData -> mContStates . mX = & rtX . lnruxliw5a [ 0
] ; simulationData -> mData -> mDiscStates . mN = 0 ; simulationData -> mData
-> mDiscStates . mX = & rtDW . awzqwdk31a ; simulationData -> mData ->
mModeVector . mN = 0 ; simulationData -> mData -> mModeVector . mX = & rtDW .
onnfopq04a ; tmp = ( ssIsMajorTimeStep ( rtS ) && ssGetRTWSolverInfo ( rtS )
-> foundContZcEvents ) ; simulationData -> mData -> mFoundZcEvents = tmp ;
simulationData -> mData -> mIsMajorTimeStep = ssIsMajorTimeStep ( rtS ) ; tmp
= ( ssGetMdlInfoPtr ( rtS ) -> mdlFlags . solverAssertCheck == 1U ) ;
simulationData -> mData -> mIsSolverAssertCheck = tmp ; tmp =
ssIsSolverCheckingCIC ( rtS ) ; simulationData -> mData ->
mIsSolverCheckingCIC = tmp ; tmp = ssIsSolverComputingJacobian ( rtS ) ;
simulationData -> mData -> mIsComputingJacobian = tmp ; simulationData ->
mData -> mIsEvaluatingF0 = ( ssGetEvaluatingF0ForJacobian ( rtS ) != 0 ) ;
tmp = ssIsSolverRequestingReset ( rtS ) ; simulationData -> mData ->
mIsSolverRequestingReset = tmp ; simulationData -> mData ->
mIsModeUpdateTimeStep = ssIsModeUpdateTimeStep ( rtS ) ; tmp_g [ 0 ] = 0 ;
tmp_m [ 0 ] = rtB . grytuooxbk [ 0 ] ; tmp_m [ 1 ] = rtB . grytuooxbk [ 1 ] ;
tmp_m [ 2 ] = rtB . grytuooxbk [ 2 ] ; tmp_m [ 3 ] = rtB . grytuooxbk [ 3 ] ;
tmp_g [ 1 ] = 4 ; simulationData -> mData -> mInputValues . mN = 4 ;
simulationData -> mData -> mInputValues . mX = & tmp_m [ 0 ] ; simulationData
-> mData -> mInputOffsets . mN = 2 ; simulationData -> mData -> mInputOffsets
. mX = & tmp_g [ 0 ] ; simulationData -> mData -> mDx . mN = 2 ;
simulationData -> mData -> mDx . mX = & _rtXdot -> lnruxliw5a [ 0 ] ;
diagnosticManager = ( NeuDiagnosticManager * ) rtDW . gzao4gihsp ;
diagnosticTree_p = neu_diagnostic_manager_get_initial_tree (
diagnosticManager ) ; tmp_i = ne_simulator_method ( ( NeslSimulator * ) rtDW
. dc0bllwdyl , NESL_SIM_FORCINGFUNCTION , simulationData , diagnosticManager
) ; if ( tmp_i != 0 ) { tmp = error_buffer_is_empty ( ssGetErrorStatus ( rtS
) ) ; if ( tmp ) { msg_p = rtw_diagnostics_msg ( diagnosticTree_p ) ;
ssSetErrorStatus ( rtS , msg_p ) ; } } simulationData = ( NeslSimulationData
* ) rtDW . fg4qcqmocx ; time_e = ssGetT ( rtS ) ; simulationData -> mData ->
mTime . mN = 1 ; simulationData -> mData -> mTime . mX = & time_e ;
simulationData -> mData -> mContStates . mN = 2 ; simulationData -> mData ->
mContStates . mX = & rtX . ipa1byflst [ 0 ] ; simulationData -> mData ->
mDiscStates . mN = 0 ; simulationData -> mData -> mDiscStates . mX = & rtDW .
grpnqhsyz0 ; simulationData -> mData -> mModeVector . mN = 0 ; simulationData
-> mData -> mModeVector . mX = & rtDW . ho0ogmrm4n ; tmp = (
ssIsMajorTimeStep ( rtS ) && ssGetRTWSolverInfo ( rtS ) -> foundContZcEvents
) ; simulationData -> mData -> mFoundZcEvents = tmp ; simulationData -> mData
-> mIsMajorTimeStep = ssIsMajorTimeStep ( rtS ) ; tmp = ( ssGetMdlInfoPtr (
rtS ) -> mdlFlags . solverAssertCheck == 1U ) ; simulationData -> mData ->
mIsSolverAssertCheck = tmp ; tmp = ssIsSolverCheckingCIC ( rtS ) ;
simulationData -> mData -> mIsSolverCheckingCIC = tmp ; tmp =
ssIsSolverComputingJacobian ( rtS ) ; simulationData -> mData ->
mIsComputingJacobian = tmp ; simulationData -> mData -> mIsEvaluatingF0 = (
ssGetEvaluatingF0ForJacobian ( rtS ) != 0 ) ; tmp = ssIsSolverRequestingReset
( rtS ) ; simulationData -> mData -> mIsSolverRequestingReset = tmp ;
simulationData -> mData -> mIsModeUpdateTimeStep = ssIsModeUpdateTimeStep (
rtS ) ; tmp_f [ 0 ] = 0 ; tmp_j [ 0 ] = rtB . n4hlwmps2s [ 0 ] ; tmp_j [ 1 ]
= rtB . n4hlwmps2s [ 1 ] ; tmp_j [ 2 ] = rtB . n4hlwmps2s [ 2 ] ; tmp_j [ 3 ]
= rtB . n4hlwmps2s [ 3 ] ; tmp_f [ 1 ] = 4 ; simulationData -> mData ->
mInputValues . mN = 4 ; simulationData -> mData -> mInputValues . mX = &
tmp_j [ 0 ] ; simulationData -> mData -> mInputOffsets . mN = 2 ;
simulationData -> mData -> mInputOffsets . mX = & tmp_f [ 0 ] ;
simulationData -> mData -> mDx . mN = 2 ; simulationData -> mData -> mDx . mX
= & _rtXdot -> ipa1byflst [ 0 ] ; diagnosticManager = ( NeuDiagnosticManager
* ) rtDW . fzld5uaeew ; diagnosticTree_e =
neu_diagnostic_manager_get_initial_tree ( diagnosticManager ) ; tmp_i =
ne_simulator_method ( ( NeslSimulator * ) rtDW . nqy5lwwfbt ,
NESL_SIM_FORCINGFUNCTION , simulationData , diagnosticManager ) ; if ( tmp_i
!= 0 ) { tmp = error_buffer_is_empty ( ssGetErrorStatus ( rtS ) ) ; if ( tmp
) { msg_e = rtw_diagnostics_msg ( diagnosticTree_e ) ; ssSetErrorStatus ( rtS
, msg_e ) ; } } } void MdlMassMatrix ( void ) { NeslSimulationData *
simulationData ; NeuDiagnosticManager * diagnosticManager ; NeuDiagnosticTree
* diagnosticTree ; NeuDiagnosticTree * diagnosticTree_p ; char * msg ; char *
msg_p ; real_T tmp_j [ 4 ] ; real_T tmp_p [ 4 ] ; real_T time ; real_T time_p
; real_T * tmp_c ; real_T * tmp_i ; real_T * tmp_m ; int32_T tmp_g ; int_T
tmp_e [ 2 ] ; int_T tmp_f [ 2 ] ; boolean_T tmp ; simulationData = (
NeslSimulationData * ) rtDW . khhoybnt1p ; time = ssGetT ( rtS ) ;
simulationData -> mData -> mTime . mN = 1 ; simulationData -> mData -> mTime
. mX = & time ; simulationData -> mData -> mContStates . mN = 2 ;
simulationData -> mData -> mContStates . mX = & rtX . lnruxliw5a [ 0 ] ;
simulationData -> mData -> mDiscStates . mN = 0 ; simulationData -> mData ->
mDiscStates . mX = & rtDW . awzqwdk31a ; simulationData -> mData ->
mModeVector . mN = 0 ; simulationData -> mData -> mModeVector . mX = & rtDW .
onnfopq04a ; tmp = ( ssIsMajorTimeStep ( rtS ) && ssGetRTWSolverInfo ( rtS )
-> foundContZcEvents ) ; simulationData -> mData -> mFoundZcEvents = tmp ;
simulationData -> mData -> mIsMajorTimeStep = ssIsMajorTimeStep ( rtS ) ; tmp
= ( ssGetMdlInfoPtr ( rtS ) -> mdlFlags . solverAssertCheck == 1U ) ;
simulationData -> mData -> mIsSolverAssertCheck = tmp ; tmp =
ssIsSolverCheckingCIC ( rtS ) ; simulationData -> mData ->
mIsSolverCheckingCIC = tmp ; tmp = ssIsSolverComputingJacobian ( rtS ) ;
simulationData -> mData -> mIsComputingJacobian = tmp ; simulationData ->
mData -> mIsEvaluatingF0 = ( ssGetEvaluatingF0ForJacobian ( rtS ) != 0 ) ;
tmp = ssIsSolverRequestingReset ( rtS ) ; simulationData -> mData ->
mIsSolverRequestingReset = tmp ; simulationData -> mData ->
mIsModeUpdateTimeStep = ssIsModeUpdateTimeStep ( rtS ) ; tmp_e [ 0 ] = 0 ;
tmp_p [ 0 ] = rtB . grytuooxbk [ 0 ] ; tmp_p [ 1 ] = rtB . grytuooxbk [ 1 ] ;
tmp_p [ 2 ] = rtB . grytuooxbk [ 2 ] ; tmp_p [ 3 ] = rtB . grytuooxbk [ 3 ] ;
tmp_e [ 1 ] = 4 ; simulationData -> mData -> mInputValues . mN = 4 ;
simulationData -> mData -> mInputValues . mX = & tmp_p [ 0 ] ; simulationData
-> mData -> mInputOffsets . mN = 2 ; simulationData -> mData -> mInputOffsets
. mX = & tmp_e [ 0 ] ; tmp_i = ssGetMassMatrixPr ( rtS ) ; tmp_m =
double_pointer_shift ( tmp_i , rtDW . nvlgkermyv ) ; simulationData -> mData
-> mMassMatrixPr . mN = 1 ; simulationData -> mData -> mMassMatrixPr . mX =
tmp_m ; diagnosticManager = ( NeuDiagnosticManager * ) rtDW . gzao4gihsp ;
diagnosticTree = neu_diagnostic_manager_get_initial_tree ( diagnosticManager
) ; tmp_g = ne_simulator_method ( ( NeslSimulator * ) rtDW . dc0bllwdyl ,
NESL_SIM_MASSMATRIX , simulationData , diagnosticManager ) ; if ( tmp_g != 0
) { tmp = error_buffer_is_empty ( ssGetErrorStatus ( rtS ) ) ; if ( tmp ) {
msg = rtw_diagnostics_msg ( diagnosticTree ) ; ssSetErrorStatus ( rtS , msg )
; } } simulationData = ( NeslSimulationData * ) rtDW . fg4qcqmocx ; time_p =
ssGetT ( rtS ) ; simulationData -> mData -> mTime . mN = 1 ; simulationData
-> mData -> mTime . mX = & time_p ; simulationData -> mData -> mContStates .
mN = 2 ; simulationData -> mData -> mContStates . mX = & rtX . ipa1byflst [ 0
] ; simulationData -> mData -> mDiscStates . mN = 0 ; simulationData -> mData
-> mDiscStates . mX = & rtDW . grpnqhsyz0 ; simulationData -> mData ->
mModeVector . mN = 0 ; simulationData -> mData -> mModeVector . mX = & rtDW .
ho0ogmrm4n ; tmp = ( ssIsMajorTimeStep ( rtS ) && ssGetRTWSolverInfo ( rtS )
-> foundContZcEvents ) ; simulationData -> mData -> mFoundZcEvents = tmp ;
simulationData -> mData -> mIsMajorTimeStep = ssIsMajorTimeStep ( rtS ) ; tmp
= ( ssGetMdlInfoPtr ( rtS ) -> mdlFlags . solverAssertCheck == 1U ) ;
simulationData -> mData -> mIsSolverAssertCheck = tmp ; tmp =
ssIsSolverCheckingCIC ( rtS ) ; simulationData -> mData ->
mIsSolverCheckingCIC = tmp ; tmp = ssIsSolverComputingJacobian ( rtS ) ;
simulationData -> mData -> mIsComputingJacobian = tmp ; simulationData ->
mData -> mIsEvaluatingF0 = ( ssGetEvaluatingF0ForJacobian ( rtS ) != 0 ) ;
tmp = ssIsSolverRequestingReset ( rtS ) ; simulationData -> mData ->
mIsSolverRequestingReset = tmp ; simulationData -> mData ->
mIsModeUpdateTimeStep = ssIsModeUpdateTimeStep ( rtS ) ; tmp_f [ 0 ] = 0 ;
tmp_j [ 0 ] = rtB . n4hlwmps2s [ 0 ] ; tmp_j [ 1 ] = rtB . n4hlwmps2s [ 1 ] ;
tmp_j [ 2 ] = rtB . n4hlwmps2s [ 2 ] ; tmp_j [ 3 ] = rtB . n4hlwmps2s [ 3 ] ;
tmp_f [ 1 ] = 4 ; simulationData -> mData -> mInputValues . mN = 4 ;
simulationData -> mData -> mInputValues . mX = & tmp_j [ 0 ] ; simulationData
-> mData -> mInputOffsets . mN = 2 ; simulationData -> mData -> mInputOffsets
. mX = & tmp_f [ 0 ] ; tmp_c = ssGetMassMatrixPr ( rtS ) ; tmp_m =
double_pointer_shift ( tmp_c , rtDW . agkmgcfe2g ) ; simulationData -> mData
-> mMassMatrixPr . mN = 1 ; simulationData -> mData -> mMassMatrixPr . mX =
tmp_m ; diagnosticManager = ( NeuDiagnosticManager * ) rtDW . fzld5uaeew ;
diagnosticTree_p = neu_diagnostic_manager_get_initial_tree (
diagnosticManager ) ; tmp_g = ne_simulator_method ( ( NeslSimulator * ) rtDW
. nqy5lwwfbt , NESL_SIM_MASSMATRIX , simulationData , diagnosticManager ) ;
if ( tmp_g != 0 ) { tmp = error_buffer_is_empty ( ssGetErrorStatus ( rtS ) )
; if ( tmp ) { msg_p = rtw_diagnostics_msg ( diagnosticTree_p ) ;
ssSetErrorStatus ( rtS , msg_p ) ; } } } void MdlTerminate ( void ) {
alyuhn31fb * obj_k ; alyuhn31fbf * obj_d ; alyuhn31fbfz * obj_g ; carpjyrt23
* obj ; ecfe5we0wc * obj_p ; lnin0tndd2 * obj_e ; m0caqfktx0 * obj_i ;
mjv2tzlyyi * obj_c ; mjv2tzlyyig * obj_l ; mjv2tzlyyigm * obj_m ; ogx4llc4ar
* obj_b ; ogx4llc4arl * obj_j ; om2wdj4oun obj_n ; om2wdj4ounp obj_f ; real_T
b_p ; int32_T b ; int32_T b_i ; int32_T e ; rt_FREE ( rtDW . om1scnv1yi .
RSimInfoPtr ) ; if ( ! rtDW . ggw1b1lkek . matlabCodegenIsDeleted ) { rtDW .
ggw1b1lkek . matlabCodegenIsDeleted = true ; } obj = & rtDW . ggw1b1lkek .
IKInternal ; if ( ! obj -> matlabCodegenIsDeleted ) { obj ->
matlabCodegenIsDeleted = true ; if ( obj -> isInitialized == 1 ) { obj ->
isInitialized = 2 ; } } obj_p = & rtDW . ggw1b1lkek . IKInternal . _pobj5 ;
if ( ! obj_p -> matlabCodegenIsDeleted ) { obj_p -> matlabCodegenIsDeleted =
true ; } obj_e = & rtDW . ggw1b1lkek . IKInternal . _pobj0 ; if ( ! obj_e ->
matlabCodegenIsDeleted ) { obj_e -> matlabCodegenIsDeleted = true ; } obj_i =
& rtDW . ggw1b1lkek . IKInternal . _pobj4 ; if ( ! obj_i ->
matlabCodegenIsDeleted ) { obj_i -> matlabCodegenIsDeleted = true ; } obj_m =
& rtDW . ggw1b1lkek . TreeInternal ; if ( ! obj_m -> matlabCodegenIsDeleted )
{ obj_m -> matlabCodegenIsDeleted = true ; } obj_g = & rtDW . ggw1b1lkek .
TreeInternal . Base ; if ( ! obj_g -> matlabCodegenIsDeleted ) { obj_g ->
matlabCodegenIsDeleted = true ; } for ( b = 0 ; b < 18 ; b ++ ) { obj_g = &
rtDW . ggw1b1lkek . TreeInternal . _pobj2 [ b ] ; if ( ! obj_g ->
matlabCodegenIsDeleted ) { obj_g -> matlabCodegenIsDeleted = true ; } } for (
b = 0 ; b < 9 ; b ++ ) { obj_g = & rtDW . ggw1b1lkek . IKInternal . _pobj2 [
b ] ; if ( ! obj_g -> matlabCodegenIsDeleted ) { obj_g ->
matlabCodegenIsDeleted = true ; } } obj_g = & rtDW . ggw1b1lkek . IKInternal
. _pobj4 . Base ; if ( ! obj_g -> matlabCodegenIsDeleted ) { obj_g ->
matlabCodegenIsDeleted = true ; } for ( b = 0 ; b < 9 ; b ++ ) { obj_g = &
rtDW . ggw1b1lkek . IKInternal . _pobj4 . _pobj0 [ b ] ; if ( ! obj_g ->
matlabCodegenIsDeleted ) { obj_g -> matlabCodegenIsDeleted = true ; } } for (
e = 0 ; e < 19 ; e ++ ) { obj_j = & rtDW . ggw1b1lkek . TreeInternal . _pobj0
[ e ] ; if ( ! obj_j -> matlabCodegenIsDeleted ) { obj_j ->
matlabCodegenIsDeleted = true ; b_p = obj_j -> Size ; b = ( int32_T ) b_p - 1
; for ( b_i = 0 ; b_i <= b ; b_i ++ ) { obj_f = obj_j -> CollisionGeometries
-> data [ b_i ] ; collisioncodegen_destructGeometry ( & obj_f .
CollisionPrimitive ) ; obj_j -> CollisionGeometries -> data [ b_i ] = obj_f ;
} } } for ( e = 0 ; e < 19 ; e ++ ) { obj_j = & rtDW . ggw1b1lkek .
IKInternal . _pobj3 [ e ] ; if ( ! obj_j -> matlabCodegenIsDeleted ) { obj_j
-> matlabCodegenIsDeleted = true ; b_p = obj_j -> Size ; b = ( int32_T ) b_p
- 1 ; for ( b_i = 0 ; b_i <= b ; b_i ++ ) { obj_f = obj_j ->
CollisionGeometries -> data [ b_i ] ; collisioncodegen_destructGeometry ( &
obj_f . CollisionPrimitive ) ; obj_j -> CollisionGeometries -> data [ b_i ] =
obj_f ; } } } for ( e = 0 ; e < 10 ; e ++ ) { obj_j = & rtDW . ggw1b1lkek .
IKInternal . _pobj4 . _pobj1 [ e ] ; if ( ! obj_j -> matlabCodegenIsDeleted )
{ obj_j -> matlabCodegenIsDeleted = true ; b_p = obj_j -> Size ; b = (
int32_T ) b_p - 1 ; for ( b_i = 0 ; b_i <= b ; b_i ++ ) { obj_f = obj_j ->
CollisionGeometries -> data [ b_i ] ; collisioncodegen_destructGeometry ( &
obj_f . CollisionPrimitive ) ; obj_j -> CollisionGeometries -> data [ b_i ] =
obj_f ; } } } omslekmpnx ( & rtDW . ggw1b1lkek ) ; if ( ! rtDW . c2ho3h3oyv .
matlabCodegenIsDeleted ) { rtDW . c2ho3h3oyv . matlabCodegenIsDeleted = true
; } obj_c = & rtDW . c2ho3h3oyv . TreeInternal ; if ( ! obj_c ->
matlabCodegenIsDeleted ) { obj_c -> matlabCodegenIsDeleted = true ; } obj_k =
& rtDW . c2ho3h3oyv . TreeInternal . Base ; if ( ! obj_k ->
matlabCodegenIsDeleted ) { obj_k -> matlabCodegenIsDeleted = true ; } for ( b
= 0 ; b < 18 ; b ++ ) { obj_k = & rtDW . c2ho3h3oyv . TreeInternal . _pobj0 [
b ] ; if ( ! obj_k -> matlabCodegenIsDeleted ) { obj_k ->
matlabCodegenIsDeleted = true ; } } obj_b = & rtDW . c2ho3h3oyv .
TreeInternal . Base . CollisionsInternal ; if ( ! obj_b ->
matlabCodegenIsDeleted ) { obj_b -> matlabCodegenIsDeleted = true ; b_p =
obj_b -> Size ; b = ( int32_T ) b_p - 1 ; for ( b_i = 0 ; b_i <= b ; b_i ++ )
{ obj_n = obj_b -> CollisionGeometries -> data [ b_i ] ;
collisioncodegen_destructGeometry ( & obj_n . CollisionPrimitive ) ; obj_b ->
CollisionGeometries -> data [ b_i ] = obj_n ; } } for ( b = 0 ; b < 18 ; b ++
) { obj_b = & rtDW . c2ho3h3oyv . TreeInternal . _pobj0 [ b ] .
CollisionsInternal ; if ( ! obj_b -> matlabCodegenIsDeleted ) { obj_b ->
matlabCodegenIsDeleted = true ; b_p = obj_b -> Size ; e = ( int32_T ) b_p - 1
; for ( b_i = 0 ; b_i <= e ; b_i ++ ) { obj_n = obj_b -> CollisionGeometries
-> data [ b_i ] ; collisioncodegen_destructGeometry ( & obj_n .
CollisionPrimitive ) ; obj_b -> CollisionGeometries -> data [ b_i ] = obj_n ;
} } } omslekmpnxk ( & rtDW . c2ho3h3oyv ) ; neu_destroy_diagnostic_manager (
( NeuDiagnosticManager * ) rtDW . caiz5lqz5f ) ; nesl_destroy_simulation_data
( ( NeslSimulationData * ) rtDW . a5qgeff343 ) ; nesl_erase_simulator (
"openManipulatorTrajectoryPlanning/Solver Configuration_1" ) ;
nesl_destroy_registry ( ) ; neu_destroy_diagnostic_manager ( (
NeuDiagnosticManager * ) rtDW . auudttxoyr ) ; nesl_destroy_simulation_data (
( NeslSimulationData * ) rtDW . papxjgo3op ) ; nesl_erase_simulator (
"openManipulatorTrajectoryPlanning/Solver Configuration_1" ) ;
nesl_destroy_registry ( ) ; if ( ! rtDW . pqz301f1cc . matlabCodegenIsDeleted
) { rtDW . pqz301f1cc . matlabCodegenIsDeleted = true ; } obj_l = & rtDW .
pqz301f1cc . TreeInternal ; if ( ! obj_l -> matlabCodegenIsDeleted ) { obj_l
-> matlabCodegenIsDeleted = true ; } obj_d = & rtDW . pqz301f1cc .
TreeInternal . Base ; if ( ! obj_d -> matlabCodegenIsDeleted ) { obj_d ->
matlabCodegenIsDeleted = true ; } for ( b = 0 ; b < 18 ; b ++ ) { obj_d = &
rtDW . pqz301f1cc . TreeInternal . _pobj0 [ b ] ; if ( ! obj_d ->
matlabCodegenIsDeleted ) { obj_d -> matlabCodegenIsDeleted = true ; } } obj_b
= & rtDW . pqz301f1cc . TreeInternal . Base . CollisionsInternal ; if ( !
obj_b -> matlabCodegenIsDeleted ) { obj_b -> matlabCodegenIsDeleted = true ;
b_p = obj_b -> Size ; b = ( int32_T ) b_p - 1 ; for ( b_i = 0 ; b_i <= b ;
b_i ++ ) { obj_n = obj_b -> CollisionGeometries -> data [ b_i ] ;
collisioncodegen_destructGeometry ( & obj_n . CollisionPrimitive ) ; obj_b ->
CollisionGeometries -> data [ b_i ] = obj_n ; } } for ( b = 0 ; b < 18 ; b ++
) { obj_b = & rtDW . pqz301f1cc . TreeInternal . _pobj0 [ b ] .
CollisionsInternal ; if ( ! obj_b -> matlabCodegenIsDeleted ) { obj_b ->
matlabCodegenIsDeleted = true ; b_p = obj_b -> Size ; e = ( int32_T ) b_p - 1
; for ( b_i = 0 ; b_i <= e ; b_i ++ ) { obj_n = obj_b -> CollisionGeometries
-> data [ b_i ] ; collisioncodegen_destructGeometry ( & obj_n .
CollisionPrimitive ) ; obj_b -> CollisionGeometries -> data [ b_i ] = obj_n ;
} } } omslekmpnxkp ( & rtDW . pqz301f1cc ) ; neu_destroy_diagnostic_manager (
( NeuDiagnosticManager * ) rtDW . gzao4gihsp ) ; nesl_destroy_simulation_data
( ( NeslSimulationData * ) rtDW . khhoybnt1p ) ; nesl_erase_simulator (
"openManipulatorTrajectoryPlanning/Solver Configuration_2" ) ;
nesl_destroy_registry ( ) ; neu_destroy_diagnostic_manager ( (
NeuDiagnosticManager * ) rtDW . mawdbjpiix ) ; nesl_destroy_simulation_data (
( NeslSimulationData * ) rtDW . m2vrnf5y3k ) ; nesl_erase_simulator (
"openManipulatorTrajectoryPlanning/Solver Configuration_2" ) ;
nesl_destroy_registry ( ) ; neu_destroy_diagnostic_manager ( (
NeuDiagnosticManager * ) rtDW . fzld5uaeew ) ; nesl_destroy_simulation_data (
( NeslSimulationData * ) rtDW . fg4qcqmocx ) ; nesl_erase_simulator (
"openManipulatorTrajectoryPlanning/Solver Configuration_3" ) ;
nesl_destroy_registry ( ) ; neu_destroy_diagnostic_manager ( (
NeuDiagnosticManager * ) rtDW . cdslitainn ) ; nesl_destroy_simulation_data (
( NeslSimulationData * ) rtDW . m3o4jlnlit ) ; nesl_erase_simulator (
"openManipulatorTrajectoryPlanning/Solver Configuration_3" ) ;
nesl_destroy_registry ( ) ; { if ( rtDW . mdhjjuxljs . AQHandles ) {
sdiTerminateStreaming ( & rtDW . mdhjjuxljs . AQHandles ) ; } } { if ( rtDW .
h42mla3rja . AQHandles ) { sdiTerminateStreaming ( & rtDW . h42mla3rja .
AQHandles ) ; } } { if ( rtDW . knk4hpmg1v . AQHandles ) {
sdiTerminateStreaming ( & rtDW . knk4hpmg1v . AQHandles ) ; } } { if ( rtDW .
hbahckh543 . AQHandles ) { sdiTerminateStreaming ( & rtDW . hbahckh543 .
AQHandles ) ; } } { if ( rtDW . oesft45bii . AQHandles ) {
sdiTerminateStreaming ( & rtDW . oesft45bii . AQHandles ) ; } } { if ( rtDW .
nzulbnsjry . AQHandles ) { sdiTerminateStreaming ( & rtDW . nzulbnsjry .
AQHandles ) ; } } { if ( rtDW . ppxa1k2ehz . AQHandles ) {
sdiTerminateStreaming ( & rtDW . ppxa1k2ehz . AQHandles ) ; } } { if ( rtDW .
axrm20twhx . AQHandles ) { sdiTerminateStreaming ( & rtDW . axrm20twhx .
AQHandles ) ; } } { if ( rtDW . dygnyfsjhq . AQHandles ) {
sdiTerminateStreaming ( & rtDW . dygnyfsjhq . AQHandles ) ; } } } static void
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( mxArray * destArray
, mwIndex i , int j , const void * srcData , size_t numBytes ) ; static void
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( mxArray * destArray
, mwIndex i , int j , const void * srcData , size_t numBytes ) { mxArray *
newArray = mxCreateUninitNumericMatrix ( ( size_t ) 1 , numBytes ,
mxUINT8_CLASS , mxREAL ) ; memcpy ( ( uint8_T * ) mxGetData ( newArray ) , (
const uint8_T * ) srcData , numBytes ) ; mxSetFieldByNumber ( destArray , i ,
j , newArray ) ; } static void
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( void * destData
, const mxArray * srcArray , mwIndex i , int j , size_t numBytes ) ; static
void mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( void *
destData , const mxArray * srcArray , mwIndex i , int j , size_t numBytes ) {
memcpy ( ( uint8_T * ) destData , ( const uint8_T * ) mxGetData (
mxGetFieldByNumber ( srcArray , i , j ) ) , numBytes ) ; } static void
mr_openManipulatorTrajectoryPlanning_cacheBitFieldToMxArray ( mxArray *
destArray , mwIndex i , int j , uint_T bitVal ) ; static void
mr_openManipulatorTrajectoryPlanning_cacheBitFieldToMxArray ( mxArray *
destArray , mwIndex i , int j , uint_T bitVal ) { mxSetFieldByNumber (
destArray , i , j , mxCreateDoubleScalar ( ( double ) bitVal ) ) ; } static
uint_T mr_openManipulatorTrajectoryPlanning_extractBitFieldFromMxArray (
const mxArray * srcArray , mwIndex i , int j , uint_T numBits ) ; static
uint_T mr_openManipulatorTrajectoryPlanning_extractBitFieldFromMxArray (
const mxArray * srcArray , mwIndex i , int j , uint_T numBits ) { const
uint_T varVal = ( uint_T ) mxGetScalar ( mxGetFieldByNumber ( srcArray , i ,
j ) ) ; return varVal & ( ( 1u << numBits ) - 1u ) ; } static void
mr_openManipulatorTrajectoryPlanning_cacheDataToMxArrayWithOffset ( mxArray *
destArray , mwIndex i , int j , mwIndex offset , const void * srcData ,
size_t numBytes ) ; static void
mr_openManipulatorTrajectoryPlanning_cacheDataToMxArrayWithOffset ( mxArray *
destArray , mwIndex i , int j , mwIndex offset , const void * srcData ,
size_t numBytes ) { uint8_T * varData = ( uint8_T * ) mxGetData (
mxGetFieldByNumber ( destArray , i , j ) ) ; memcpy ( ( uint8_T * ) & varData
[ offset * numBytes ] , ( const uint8_T * ) srcData , numBytes ) ; } static
void mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArrayWithOffset (
void * destData , const mxArray * srcArray , mwIndex i , int j , mwIndex
offset , size_t numBytes ) ; static void
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArrayWithOffset ( void
* destData , const mxArray * srcArray , mwIndex i , int j , mwIndex offset ,
size_t numBytes ) { const uint8_T * varData = ( const uint8_T * ) mxGetData (
mxGetFieldByNumber ( srcArray , i , j ) ) ; memcpy ( ( uint8_T * ) destData ,
( const uint8_T * ) & varData [ offset * numBytes ] , numBytes ) ; } static
void mr_openManipulatorTrajectoryPlanning_cacheBitFieldToCellArrayWithOffset
( mxArray * destArray , mwIndex i , int j , mwIndex offset , uint_T fieldVal
) ; static void
mr_openManipulatorTrajectoryPlanning_cacheBitFieldToCellArrayWithOffset (
mxArray * destArray , mwIndex i , int j , mwIndex offset , uint_T fieldVal )
{ mxSetCell ( mxGetFieldByNumber ( destArray , i , j ) , offset ,
mxCreateDoubleScalar ( ( double ) fieldVal ) ) ; } static uint_T
mr_openManipulatorTrajectoryPlanning_extractBitFieldFromCellArrayWithOffset (
const mxArray * srcArray , mwIndex i , int j , mwIndex offset , uint_T
numBits ) ; static uint_T
mr_openManipulatorTrajectoryPlanning_extractBitFieldFromCellArrayWithOffset (
const mxArray * srcArray , mwIndex i , int j , mwIndex offset , uint_T
numBits ) { const uint_T fieldVal = ( uint_T ) mxGetScalar ( mxGetCell (
mxGetFieldByNumber ( srcArray , i , j ) , offset ) ) ; return fieldVal & ( (
1u << numBits ) - 1u ) ; } mxArray *
mr_openManipulatorTrajectoryPlanning_GetDWork ( ) { static const char *
ssDWFieldNames [ 3 ] = { "rtB" , "rtDW" , "NULL_PrevZCX" , } ; mxArray * ssDW
= mxCreateStructMatrix ( 1 , 1 , 3 , ssDWFieldNames ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( ssDW , 0 , 0 , (
const void * ) & ( rtB ) , sizeof ( rtB ) ) ; { static const char *
rtdwDataFieldNames [ 72 ] = { "rtDW.ggw1b1lkek" , "rtDW.pqz301f1cc" ,
"rtDW.c2ho3h3oyv" , "rtDW.jiqh55j12p" , "rtDW.ahio0mgx5h" , "rtDW.jmlpsyjpqa"
, "rtDW.osgjdwjesl" , "rtDW.fkcgzk5oma" , "rtDW.c45aidngho" ,
"rtDW.kaentdmtdn" , "rtDW.d5wogx3kqa" , "rtDW.ggzlanf4wa" , "rtDW.p4ux5yfv5l"
, "rtDW.jhwzr3b1pg" , "rtDW.eukf1npmbe" , "rtDW.eutf4o2lqe" ,
"rtDW.dwwiwqgqz0" , "rtDW.pmpxho4tev" , "rtDW.eqedaoza0o" , "rtDW.bgoyflaz0e"
, "rtDW.liwevcrd2d" , "rtDW.a4s0ocg4tv" , "rtDW.kqczznesoc" ,
"rtDW.ac1ikcnlza" , "rtDW.mtraeigdsz" , "rtDW.g4g3vrgle4" , "rtDW.ofd1yxgqn2"
, "rtDW.imqlsd2301" , "rtDW.jqx3umoi0g" , "rtDW.bwwbiride4" ,
"rtDW.axm3re1vxp" , "rtDW.ap5zbvosy0" , "rtDW.awzqwdk31a" , "rtDW.f0i5qqud1u"
, "rtDW.grpnqhsyz0" , "rtDW.h0rypimiph" , "rtDW.ccbb0ktavq" ,
"rtDW.iujlyug3z5" , "rtDW.bhbuxnbarg" , "rtDW.ehmyrdjr0v" , "rtDW.m2usyxvuaa"
, "rtDW.hl2ip4fyfs" , "rtDW.kad3ljw5be" , "rtDW.f0qx01hjkp" ,
"rtDW.jffwzd0nlb" , "rtDW.l1cob02vt3" , "rtDW.edj0t2oelw" , "rtDW.arxdbmacvj"
, "rtDW.onnfopq04a" , "rtDW.mzl33nqclq" , "rtDW.ho0ogmrm4n" ,
"rtDW.k0v0fvu5ab" , "rtDW.nvlgkermyv" , "rtDW.agkmgcfe2g" , "rtDW.elw5mlszj3"
, "rtDW.hgzbqwt1rw" , "rtDW.fjiyb4fazm" , "rtDW.ammknauxgb" ,
"rtDW.ju2keuyi2y" , "rtDW.ilkhrfgy3p" , "rtDW.kta541utr1" , "rtDW.awistkf5gl"
, "rtDW.ohnifdbmla" , "rtDW.kgbrg3ivnd" , "rtDW.ejmahq1kwn" ,
"rtDW.gcwd0zaucf" , "rtDW.jvss2h1o3w" , "rtDW.jmldnbipjd" , "rtDW.n0rsfsdi3c"
, "rtDW.pxayrmk0dv" , "rtDW.l11orfjj1u" , "rtDW.nervxud5ud" , } ; mxArray *
rtdwData = mxCreateStructMatrix ( 1 , 1 , 72 , rtdwDataFieldNames ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 0 ,
( const void * ) & ( rtDW . ggw1b1lkek ) , sizeof ( rtDW . ggw1b1lkek ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 1 ,
( const void * ) & ( rtDW . pqz301f1cc ) , sizeof ( rtDW . pqz301f1cc ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 2 ,
( const void * ) & ( rtDW . c2ho3h3oyv ) , sizeof ( rtDW . c2ho3h3oyv ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 3 ,
( const void * ) & ( rtDW . jiqh55j12p ) , sizeof ( rtDW . jiqh55j12p ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 4 ,
( const void * ) & ( rtDW . ahio0mgx5h ) , sizeof ( rtDW . ahio0mgx5h ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 5 ,
( const void * ) & ( rtDW . jmlpsyjpqa ) , sizeof ( rtDW . jmlpsyjpqa ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 6 ,
( const void * ) & ( rtDW . osgjdwjesl ) , sizeof ( rtDW . osgjdwjesl ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 7 ,
( const void * ) & ( rtDW . fkcgzk5oma ) , sizeof ( rtDW . fkcgzk5oma ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 8 ,
( const void * ) & ( rtDW . c45aidngho ) , sizeof ( rtDW . c45aidngho ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 9 ,
( const void * ) & ( rtDW . kaentdmtdn ) , sizeof ( rtDW . kaentdmtdn ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 10 ,
( const void * ) & ( rtDW . d5wogx3kqa ) , sizeof ( rtDW . d5wogx3kqa ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 11 ,
( const void * ) & ( rtDW . ggzlanf4wa ) , sizeof ( rtDW . ggzlanf4wa ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 12 ,
( const void * ) & ( rtDW . p4ux5yfv5l ) , sizeof ( rtDW . p4ux5yfv5l ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 13 ,
( const void * ) & ( rtDW . jhwzr3b1pg ) , sizeof ( rtDW . jhwzr3b1pg ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 14 ,
( const void * ) & ( rtDW . eukf1npmbe ) , sizeof ( rtDW . eukf1npmbe ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 15 ,
( const void * ) & ( rtDW . eutf4o2lqe ) , sizeof ( rtDW . eutf4o2lqe ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 16 ,
( const void * ) & ( rtDW . dwwiwqgqz0 ) , sizeof ( rtDW . dwwiwqgqz0 ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 17 ,
( const void * ) & ( rtDW . pmpxho4tev ) , sizeof ( rtDW . pmpxho4tev ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 18 ,
( const void * ) & ( rtDW . eqedaoza0o ) , sizeof ( rtDW . eqedaoza0o ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 19 ,
( const void * ) & ( rtDW . bgoyflaz0e ) , sizeof ( rtDW . bgoyflaz0e ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 20 ,
( const void * ) & ( rtDW . liwevcrd2d ) , sizeof ( rtDW . liwevcrd2d ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 21 ,
( const void * ) & ( rtDW . a4s0ocg4tv ) , sizeof ( rtDW . a4s0ocg4tv ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 22 ,
( const void * ) & ( rtDW . kqczznesoc ) , sizeof ( rtDW . kqczznesoc ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 23 ,
( const void * ) & ( rtDW . ac1ikcnlza ) , sizeof ( rtDW . ac1ikcnlza ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 24 ,
( const void * ) & ( rtDW . mtraeigdsz ) , sizeof ( rtDW . mtraeigdsz ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 25 ,
( const void * ) & ( rtDW . g4g3vrgle4 ) , sizeof ( rtDW . g4g3vrgle4 ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 26 ,
( const void * ) & ( rtDW . ofd1yxgqn2 ) , sizeof ( rtDW . ofd1yxgqn2 ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 27 ,
( const void * ) & ( rtDW . imqlsd2301 ) , sizeof ( rtDW . imqlsd2301 ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 28 ,
( const void * ) & ( rtDW . jqx3umoi0g ) , sizeof ( rtDW . jqx3umoi0g ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 29 ,
( const void * ) & ( rtDW . bwwbiride4 ) , sizeof ( rtDW . bwwbiride4 ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 30 ,
( const void * ) & ( rtDW . axm3re1vxp ) , sizeof ( rtDW . axm3re1vxp ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 31 ,
( const void * ) & ( rtDW . ap5zbvosy0 ) , sizeof ( rtDW . ap5zbvosy0 ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 32 ,
( const void * ) & ( rtDW . awzqwdk31a ) , sizeof ( rtDW . awzqwdk31a ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 33 ,
( const void * ) & ( rtDW . f0i5qqud1u ) , sizeof ( rtDW . f0i5qqud1u ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 34 ,
( const void * ) & ( rtDW . grpnqhsyz0 ) , sizeof ( rtDW . grpnqhsyz0 ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 35 ,
( const void * ) & ( rtDW . h0rypimiph ) , sizeof ( rtDW . h0rypimiph ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 36 ,
( const void * ) & ( rtDW . ccbb0ktavq ) , sizeof ( rtDW . ccbb0ktavq ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 37 ,
( const void * ) & ( rtDW . iujlyug3z5 ) , sizeof ( rtDW . iujlyug3z5 ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 38 ,
( const void * ) & ( rtDW . bhbuxnbarg ) , sizeof ( rtDW . bhbuxnbarg ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 39 ,
( const void * ) & ( rtDW . ehmyrdjr0v ) , sizeof ( rtDW . ehmyrdjr0v ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 40 ,
( const void * ) & ( rtDW . m2usyxvuaa ) , sizeof ( rtDW . m2usyxvuaa ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 41 ,
( const void * ) & ( rtDW . hl2ip4fyfs ) , sizeof ( rtDW . hl2ip4fyfs ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 42 ,
( const void * ) & ( rtDW . kad3ljw5be ) , sizeof ( rtDW . kad3ljw5be ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 43 ,
( const void * ) & ( rtDW . f0qx01hjkp ) , sizeof ( rtDW . f0qx01hjkp ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 44 ,
( const void * ) & ( rtDW . jffwzd0nlb ) , sizeof ( rtDW . jffwzd0nlb ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 45 ,
( const void * ) & ( rtDW . l1cob02vt3 ) , sizeof ( rtDW . l1cob02vt3 ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 46 ,
( const void * ) & ( rtDW . edj0t2oelw ) , sizeof ( rtDW . edj0t2oelw ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 47 ,
( const void * ) & ( rtDW . arxdbmacvj ) , sizeof ( rtDW . arxdbmacvj ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 48 ,
( const void * ) & ( rtDW . onnfopq04a ) , sizeof ( rtDW . onnfopq04a ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 49 ,
( const void * ) & ( rtDW . mzl33nqclq ) , sizeof ( rtDW . mzl33nqclq ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 50 ,
( const void * ) & ( rtDW . ho0ogmrm4n ) , sizeof ( rtDW . ho0ogmrm4n ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 51 ,
( const void * ) & ( rtDW . k0v0fvu5ab ) , sizeof ( rtDW . k0v0fvu5ab ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 52 ,
( const void * ) & ( rtDW . nvlgkermyv ) , sizeof ( rtDW . nvlgkermyv ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 53 ,
( const void * ) & ( rtDW . agkmgcfe2g ) , sizeof ( rtDW . agkmgcfe2g ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 54 ,
( const void * ) & ( rtDW . elw5mlszj3 ) , sizeof ( rtDW . elw5mlszj3 ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 55 ,
( const void * ) & ( rtDW . hgzbqwt1rw ) , sizeof ( rtDW . hgzbqwt1rw ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 56 ,
( const void * ) & ( rtDW . fjiyb4fazm ) , sizeof ( rtDW . fjiyb4fazm ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 57 ,
( const void * ) & ( rtDW . ammknauxgb ) , sizeof ( rtDW . ammknauxgb ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 58 ,
( const void * ) & ( rtDW . ju2keuyi2y ) , sizeof ( rtDW . ju2keuyi2y ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 59 ,
( const void * ) & ( rtDW . ilkhrfgy3p ) , sizeof ( rtDW . ilkhrfgy3p ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 60 ,
( const void * ) & ( rtDW . kta541utr1 ) , sizeof ( rtDW . kta541utr1 ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 61 ,
( const void * ) & ( rtDW . awistkf5gl ) , sizeof ( rtDW . awistkf5gl ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 62 ,
( const void * ) & ( rtDW . ohnifdbmla ) , sizeof ( rtDW . ohnifdbmla ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 63 ,
( const void * ) & ( rtDW . kgbrg3ivnd ) , sizeof ( rtDW . kgbrg3ivnd ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 64 ,
( const void * ) & ( rtDW . ejmahq1kwn ) , sizeof ( rtDW . ejmahq1kwn ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 65 ,
( const void * ) & ( rtDW . gcwd0zaucf ) , sizeof ( rtDW . gcwd0zaucf ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 66 ,
( const void * ) & ( rtDW . jvss2h1o3w ) , sizeof ( rtDW . jvss2h1o3w ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 67 ,
( const void * ) & ( rtDW . jmldnbipjd ) , sizeof ( rtDW . jmldnbipjd ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 68 ,
( const void * ) & ( rtDW . n0rsfsdi3c ) , sizeof ( rtDW . n0rsfsdi3c ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 69 ,
( const void * ) & ( rtDW . pxayrmk0dv ) , sizeof ( rtDW . pxayrmk0dv ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 70 ,
( const void * ) & ( rtDW . l11orfjj1u ) , sizeof ( rtDW . l11orfjj1u ) ) ;
mr_openManipulatorTrajectoryPlanning_cacheDataAsMxArray ( rtdwData , 0 , 71 ,
( const void * ) & ( rtDW . nervxud5ud ) , sizeof ( rtDW . nervxud5ud ) ) ;
mxSetFieldByNumber ( ssDW , 0 , 1 , rtdwData ) ; } return ssDW ; } void
mr_openManipulatorTrajectoryPlanning_SetDWork ( const mxArray * ssDW ) { (
void ) ssDW ; mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( (
void * ) & ( rtB ) , ssDW , 0 , 0 , sizeof ( rtB ) ) ; { const mxArray *
rtdwData = mxGetFieldByNumber ( ssDW , 0 , 1 ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . ggw1b1lkek ) , rtdwData , 0 , 0 , sizeof ( rtDW . ggw1b1lkek ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . pqz301f1cc ) , rtdwData , 0 , 1 , sizeof ( rtDW . pqz301f1cc ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . c2ho3h3oyv ) , rtdwData , 0 , 2 , sizeof ( rtDW . c2ho3h3oyv ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . jiqh55j12p ) , rtdwData , 0 , 3 , sizeof ( rtDW . jiqh55j12p ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . ahio0mgx5h ) , rtdwData , 0 , 4 , sizeof ( rtDW . ahio0mgx5h ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . jmlpsyjpqa ) , rtdwData , 0 , 5 , sizeof ( rtDW . jmlpsyjpqa ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . osgjdwjesl ) , rtdwData , 0 , 6 , sizeof ( rtDW . osgjdwjesl ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . fkcgzk5oma ) , rtdwData , 0 , 7 , sizeof ( rtDW . fkcgzk5oma ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . c45aidngho ) , rtdwData , 0 , 8 , sizeof ( rtDW . c45aidngho ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . kaentdmtdn ) , rtdwData , 0 , 9 , sizeof ( rtDW . kaentdmtdn ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . d5wogx3kqa ) , rtdwData , 0 , 10 , sizeof ( rtDW . d5wogx3kqa ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . ggzlanf4wa ) , rtdwData , 0 , 11 , sizeof ( rtDW . ggzlanf4wa ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . p4ux5yfv5l ) , rtdwData , 0 , 12 , sizeof ( rtDW . p4ux5yfv5l ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . jhwzr3b1pg ) , rtdwData , 0 , 13 , sizeof ( rtDW . jhwzr3b1pg ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . eukf1npmbe ) , rtdwData , 0 , 14 , sizeof ( rtDW . eukf1npmbe ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . eutf4o2lqe ) , rtdwData , 0 , 15 , sizeof ( rtDW . eutf4o2lqe ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . dwwiwqgqz0 ) , rtdwData , 0 , 16 , sizeof ( rtDW . dwwiwqgqz0 ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . pmpxho4tev ) , rtdwData , 0 , 17 , sizeof ( rtDW . pmpxho4tev ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . eqedaoza0o ) , rtdwData , 0 , 18 , sizeof ( rtDW . eqedaoza0o ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . bgoyflaz0e ) , rtdwData , 0 , 19 , sizeof ( rtDW . bgoyflaz0e ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . liwevcrd2d ) , rtdwData , 0 , 20 , sizeof ( rtDW . liwevcrd2d ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . a4s0ocg4tv ) , rtdwData , 0 , 21 , sizeof ( rtDW . a4s0ocg4tv ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . kqczznesoc ) , rtdwData , 0 , 22 , sizeof ( rtDW . kqczznesoc ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . ac1ikcnlza ) , rtdwData , 0 , 23 , sizeof ( rtDW . ac1ikcnlza ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . mtraeigdsz ) , rtdwData , 0 , 24 , sizeof ( rtDW . mtraeigdsz ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . g4g3vrgle4 ) , rtdwData , 0 , 25 , sizeof ( rtDW . g4g3vrgle4 ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . ofd1yxgqn2 ) , rtdwData , 0 , 26 , sizeof ( rtDW . ofd1yxgqn2 ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . imqlsd2301 ) , rtdwData , 0 , 27 , sizeof ( rtDW . imqlsd2301 ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . jqx3umoi0g ) , rtdwData , 0 , 28 , sizeof ( rtDW . jqx3umoi0g ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . bwwbiride4 ) , rtdwData , 0 , 29 , sizeof ( rtDW . bwwbiride4 ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . axm3re1vxp ) , rtdwData , 0 , 30 , sizeof ( rtDW . axm3re1vxp ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . ap5zbvosy0 ) , rtdwData , 0 , 31 , sizeof ( rtDW . ap5zbvosy0 ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . awzqwdk31a ) , rtdwData , 0 , 32 , sizeof ( rtDW . awzqwdk31a ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . f0i5qqud1u ) , rtdwData , 0 , 33 , sizeof ( rtDW . f0i5qqud1u ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . grpnqhsyz0 ) , rtdwData , 0 , 34 , sizeof ( rtDW . grpnqhsyz0 ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . h0rypimiph ) , rtdwData , 0 , 35 , sizeof ( rtDW . h0rypimiph ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . ccbb0ktavq ) , rtdwData , 0 , 36 , sizeof ( rtDW . ccbb0ktavq ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . iujlyug3z5 ) , rtdwData , 0 , 37 , sizeof ( rtDW . iujlyug3z5 ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . bhbuxnbarg ) , rtdwData , 0 , 38 , sizeof ( rtDW . bhbuxnbarg ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . ehmyrdjr0v ) , rtdwData , 0 , 39 , sizeof ( rtDW . ehmyrdjr0v ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . m2usyxvuaa ) , rtdwData , 0 , 40 , sizeof ( rtDW . m2usyxvuaa ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . hl2ip4fyfs ) , rtdwData , 0 , 41 , sizeof ( rtDW . hl2ip4fyfs ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . kad3ljw5be ) , rtdwData , 0 , 42 , sizeof ( rtDW . kad3ljw5be ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . f0qx01hjkp ) , rtdwData , 0 , 43 , sizeof ( rtDW . f0qx01hjkp ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . jffwzd0nlb ) , rtdwData , 0 , 44 , sizeof ( rtDW . jffwzd0nlb ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . l1cob02vt3 ) , rtdwData , 0 , 45 , sizeof ( rtDW . l1cob02vt3 ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . edj0t2oelw ) , rtdwData , 0 , 46 , sizeof ( rtDW . edj0t2oelw ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . arxdbmacvj ) , rtdwData , 0 , 47 , sizeof ( rtDW . arxdbmacvj ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . onnfopq04a ) , rtdwData , 0 , 48 , sizeof ( rtDW . onnfopq04a ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . mzl33nqclq ) , rtdwData , 0 , 49 , sizeof ( rtDW . mzl33nqclq ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . ho0ogmrm4n ) , rtdwData , 0 , 50 , sizeof ( rtDW . ho0ogmrm4n ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . k0v0fvu5ab ) , rtdwData , 0 , 51 , sizeof ( rtDW . k0v0fvu5ab ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . nvlgkermyv ) , rtdwData , 0 , 52 , sizeof ( rtDW . nvlgkermyv ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . agkmgcfe2g ) , rtdwData , 0 , 53 , sizeof ( rtDW . agkmgcfe2g ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . elw5mlszj3 ) , rtdwData , 0 , 54 , sizeof ( rtDW . elw5mlszj3 ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . hgzbqwt1rw ) , rtdwData , 0 , 55 , sizeof ( rtDW . hgzbqwt1rw ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . fjiyb4fazm ) , rtdwData , 0 , 56 , sizeof ( rtDW . fjiyb4fazm ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . ammknauxgb ) , rtdwData , 0 , 57 , sizeof ( rtDW . ammknauxgb ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . ju2keuyi2y ) , rtdwData , 0 , 58 , sizeof ( rtDW . ju2keuyi2y ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . ilkhrfgy3p ) , rtdwData , 0 , 59 , sizeof ( rtDW . ilkhrfgy3p ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . kta541utr1 ) , rtdwData , 0 , 60 , sizeof ( rtDW . kta541utr1 ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . awistkf5gl ) , rtdwData , 0 , 61 , sizeof ( rtDW . awistkf5gl ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . ohnifdbmla ) , rtdwData , 0 , 62 , sizeof ( rtDW . ohnifdbmla ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . kgbrg3ivnd ) , rtdwData , 0 , 63 , sizeof ( rtDW . kgbrg3ivnd ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . ejmahq1kwn ) , rtdwData , 0 , 64 , sizeof ( rtDW . ejmahq1kwn ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . gcwd0zaucf ) , rtdwData , 0 , 65 , sizeof ( rtDW . gcwd0zaucf ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . jvss2h1o3w ) , rtdwData , 0 , 66 , sizeof ( rtDW . jvss2h1o3w ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . jmldnbipjd ) , rtdwData , 0 , 67 , sizeof ( rtDW . jmldnbipjd ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . n0rsfsdi3c ) , rtdwData , 0 , 68 , sizeof ( rtDW . n0rsfsdi3c ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . pxayrmk0dv ) , rtdwData , 0 , 69 , sizeof ( rtDW . pxayrmk0dv ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . l11orfjj1u ) , rtdwData , 0 , 70 , sizeof ( rtDW . l11orfjj1u ) ) ;
mr_openManipulatorTrajectoryPlanning_restoreDataFromMxArray ( ( void * ) & (
rtDW . nervxud5ud ) , rtdwData , 0 , 71 , sizeof ( rtDW . nervxud5ud ) ) ; }
} mxArray * mr_openManipulatorTrajectoryPlanning_GetSimStateDisallowedBlocks
( ) { mxArray * data = mxCreateCellMatrix ( 10 , 3 ) ; mwIndex subs [ 2 ] ,
offset ; { static const char * blockType [ 10 ] = { "Scope" , "Scope" ,
"SimscapeExecutionBlock" , "SimscapeExecutionBlock" , "Scope" ,
"SimscapeExecutionBlock" , "SimscapeExecutionBlock" ,
"SimscapeExecutionBlock" , "SimscapeExecutionBlock" , "SimscapeSinkBlock" , }
; static const char * blockPath [ 10 ] = {
"openManipulatorTrajectoryPlanning/Scope" ,
"openManipulatorTrajectoryPlanning/Scope1" ,
"openManipulatorTrajectoryPlanning/Solver Configuration/EVAL_KEY/STATE_1" ,
"openManipulatorTrajectoryPlanning/Solver Configuration/EVAL_KEY/OUTPUT_1_0"
, "openManipulatorTrajectoryPlanning/Scope2" ,
"openManipulatorTrajectoryPlanning/Solver Configuration/EVAL_KEY/STATE_2" ,
"openManipulatorTrajectoryPlanning/Solver Configuration/EVAL_KEY/OUTPUT_2_0"
, "openManipulatorTrajectoryPlanning/Solver Configuration/EVAL_KEY/STATE_3" ,
"openManipulatorTrajectoryPlanning/Solver Configuration/EVAL_KEY/OUTPUT_3_0"
, "openManipulatorTrajectoryPlanning/Solver Configuration/EVAL_KEY/SINK_1" ,
} ; static const int reason [ 10 ] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0
, } ; for ( subs [ 0 ] = 0 ; subs [ 0 ] < 10 ; ++ ( subs [ 0 ] ) ) { subs [ 1
] = 0 ; offset = mxCalcSingleSubscript ( data , 2 , subs ) ; mxSetCell ( data
, offset , mxCreateString ( blockType [ subs [ 0 ] ] ) ) ; subs [ 1 ] = 1 ;
offset = mxCalcSingleSubscript ( data , 2 , subs ) ; mxSetCell ( data ,
offset , mxCreateString ( blockPath [ subs [ 0 ] ] ) ) ; subs [ 1 ] = 2 ;
offset = mxCalcSingleSubscript ( data , 2 , subs ) ; mxSetCell ( data ,
offset , mxCreateDoubleScalar ( ( double ) reason [ subs [ 0 ] ] ) ) ; } }
return data ; } void MdlInitializeSizes ( void ) { ssSetNumContStates ( rtS ,
16 ) ; ssSetNumPeriodicContStates ( rtS , 0 ) ; ssSetNumY ( rtS , 0 ) ;
ssSetNumU ( rtS , 0 ) ; ssSetDirectFeedThrough ( rtS , 0 ) ;
ssSetNumSampleTimes ( rtS , 3 ) ; ssSetNumBlocks ( rtS , 262 ) ;
ssSetNumBlockIO ( rtS , 37 ) ; ssSetNumBlockParams ( rtS , 130 ) ; } void
MdlInitializeSampleTimes ( void ) { ssSetSampleTime ( rtS , 0 , 0.0 ) ;
ssSetSampleTime ( rtS , 1 , 0.0 ) ; ssSetSampleTime ( rtS , 2 , 0.005 ) ;
ssSetOffsetTime ( rtS , 0 , 0.0 ) ; ssSetOffsetTime ( rtS , 1 , 1.0 ) ;
ssSetOffsetTime ( rtS , 2 , 0.0 ) ; } void raccel_set_checksum ( ) {
ssSetChecksumVal ( rtS , 0 , 791975065U ) ; ssSetChecksumVal ( rtS , 1 ,
2127965696U ) ; ssSetChecksumVal ( rtS , 2 , 1703960231U ) ; ssSetChecksumVal
( rtS , 3 , 721157778U ) ; }
#if defined(_MSC_VER)
#pragma optimize( "", off )
#endif
SimStruct * raccel_register_model ( ssExecutionInfo * executionInfo ) {
static struct _ssMdlInfo mdlInfo ; static struct _ssBlkInfo2 blkInfo2 ;
static struct _ssBlkInfoSLSize blkInfoSLSize ; ( void ) memset ( ( char * )
rtS , 0 , sizeof ( SimStruct ) ) ; ( void ) memset ( ( char * ) & mdlInfo , 0
, sizeof ( struct _ssMdlInfo ) ) ; ( void ) memset ( ( char * ) & blkInfo2 ,
0 , sizeof ( struct _ssBlkInfo2 ) ) ; ( void ) memset ( ( char * ) &
blkInfoSLSize , 0 , sizeof ( struct _ssBlkInfoSLSize ) ) ; ssSetBlkInfo2Ptr (
rtS , & blkInfo2 ) ; ssSetBlkInfoSLSizePtr ( rtS , & blkInfoSLSize ) ;
ssSetMdlInfoPtr ( rtS , & mdlInfo ) ; ssSetExecutionInfo ( rtS ,
executionInfo ) ; slsaAllocOPModelData ( rtS ) ; { static time_T mdlPeriod [
NSAMPLE_TIMES ] ; static time_T mdlOffset [ NSAMPLE_TIMES ] ; static time_T
mdlTaskTimes [ NSAMPLE_TIMES ] ; static int_T mdlTsMap [ NSAMPLE_TIMES ] ;
static int_T mdlSampleHits [ NSAMPLE_TIMES ] ; static boolean_T
mdlTNextWasAdjustedPtr [ NSAMPLE_TIMES ] ; static int_T mdlPerTaskSampleHits
[ NSAMPLE_TIMES * NSAMPLE_TIMES ] ; static time_T mdlTimeOfNextSampleHit [
NSAMPLE_TIMES ] ; { int_T i ; for ( i = 0 ; i < NSAMPLE_TIMES ; i ++ ) {
mdlPeriod [ i ] = 0.0 ; mdlOffset [ i ] = 0.0 ; mdlTaskTimes [ i ] = 0.0 ;
mdlTsMap [ i ] = i ; mdlSampleHits [ i ] = 1 ; } } ssSetSampleTimePtr ( rtS ,
& mdlPeriod [ 0 ] ) ; ssSetOffsetTimePtr ( rtS , & mdlOffset [ 0 ] ) ;
ssSetSampleTimeTaskIDPtr ( rtS , & mdlTsMap [ 0 ] ) ; ssSetTPtr ( rtS , &
mdlTaskTimes [ 0 ] ) ; ssSetSampleHitPtr ( rtS , & mdlSampleHits [ 0 ] ) ;
ssSetTNextWasAdjustedPtr ( rtS , & mdlTNextWasAdjustedPtr [ 0 ] ) ;
ssSetPerTaskSampleHitsPtr ( rtS , & mdlPerTaskSampleHits [ 0 ] ) ;
ssSetTimeOfNextSampleHitPtr ( rtS , & mdlTimeOfNextSampleHit [ 0 ] ) ; }
ssSetSolverMode ( rtS , SOLVER_MODE_SINGLETASKING ) ; { ssSetBlockIO ( rtS ,
( ( void * ) & rtB ) ) ; ( void ) memset ( ( ( void * ) & rtB ) , 0 , sizeof
( B ) ) ; } { real_T * x = ( real_T * ) & rtX ; ssSetContStates ( rtS , x ) ;
( void ) memset ( ( void * ) x , 0 , sizeof ( X ) ) ; } { void * dwork = (
void * ) & rtDW ; ssSetRootDWork ( rtS , dwork ) ; ( void ) memset ( dwork ,
0 , sizeof ( DW ) ) ; } { rtDW . nvlgkermyv = 12 ; rtDW . agkmgcfe2g = 13 ; }
{ static DataTypeTransInfo dtInfo ; ( void ) memset ( ( char_T * ) & dtInfo ,
0 , sizeof ( dtInfo ) ) ; ssSetModelMappingInfo ( rtS , & dtInfo ) ; dtInfo .
numDataTypes = 29 ; dtInfo . dataTypeSizes = & rtDataTypeSizes [ 0 ] ; dtInfo
. dataTypeNames = & rtDataTypeNames [ 0 ] ; dtInfo . BTransTable = &
rtBTransTable ; dtInfo . PTransTable = & rtPTransTable ; dtInfo .
dataTypeInfoTable = rtDataTypeInfoTable ; }
openManipulatorTrajectoryPlanning_InitializeDataMapInfo ( ) ;
ssSetIsRapidAcceleratorActive ( rtS , true ) ; ssSetRootSS ( rtS , rtS ) ;
ssSetVersion ( rtS , SIMSTRUCT_VERSION_LEVEL2 ) ; ssSetModelName ( rtS ,
"openManipulatorTrajectoryPlanning" ) ; ssSetPath ( rtS ,
"openManipulatorTrajectoryPlanning" ) ; ssSetTStart ( rtS , 0.0 ) ;
ssSetTFinal ( rtS , 2.0 ) ; { static RTWLogInfo rt_DataLoggingInfo ;
rt_DataLoggingInfo . loggingInterval = ( NULL ) ; ssSetRTWLogInfo ( rtS , &
rt_DataLoggingInfo ) ; } { { static int_T rt_LoggedStateWidths [ ] = { 1 , 1
, 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 6 , 4 , 1 , 1 , 1 ,
1 , 1 , 1 , 6 , 6 , 2 , 2 , 2 , 2 , 2 , 2 , 2 , 2 , 2 , 2 , 2 , 2 } ; static
int_T rt_LoggedStateNumDimensions [ ] = { 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 ,
1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1
, 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 } ; static int_T rt_LoggedStateDimensions
[ ] = { 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 6 , 4
, 1 , 1 , 1 , 1 , 1 , 1 , 6 , 6 , 2 , 2 , 2 , 2 , 2 , 2 , 2 , 2 , 2 , 2 , 2 ,
2 } ; static boolean_T rt_LoggedStateIsVarDims [ ] = { 0 , 0 , 0 , 0 , 0 , 0
, 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 } ; static BuiltInDTypeId
rt_LoggedStateDataTypeIds [ ] = { SS_DOUBLE , SS_DOUBLE , SS_DOUBLE ,
SS_DOUBLE , SS_DOUBLE , SS_DOUBLE , SS_DOUBLE , SS_DOUBLE , SS_DOUBLE ,
SS_DOUBLE , SS_DOUBLE , SS_DOUBLE , SS_DOUBLE , SS_DOUBLE , SS_DOUBLE ,
SS_DOUBLE , SS_DOUBLE , SS_DOUBLE , SS_DOUBLE , SS_DOUBLE , SS_DOUBLE ,
SS_DOUBLE , SS_DOUBLE , SS_DOUBLE , SS_DOUBLE , SS_DOUBLE , SS_DOUBLE ,
SS_DOUBLE , SS_DOUBLE , SS_DOUBLE , SS_DOUBLE , SS_DOUBLE , SS_DOUBLE ,
SS_DOUBLE , SS_DOUBLE , SS_DOUBLE , SS_DOUBLE , SS_DOUBLE } ; static int_T
rt_LoggedStateComplexSignals [ ] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0
, 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 } ; static RTWPreprocessingFcnPtr
rt_LoggingStatePreprocessingFcnPtrs [ ] = { ( NULL ) , ( NULL ) , ( NULL ) ,
( NULL ) , ( NULL ) , ( NULL ) , ( NULL ) , ( NULL ) , ( NULL ) , ( NULL ) ,
( NULL ) , ( NULL ) , ( NULL ) , ( NULL ) , ( NULL ) , ( NULL ) , ( NULL ) ,
( NULL ) , ( NULL ) , ( NULL ) , ( NULL ) , ( NULL ) , ( NULL ) , ( NULL ) ,
( NULL ) , ( NULL ) , ( NULL ) , ( NULL ) , ( NULL ) , ( NULL ) , ( NULL ) ,
( NULL ) , ( NULL ) , ( NULL ) , ( NULL ) , ( NULL ) , ( NULL ) , ( NULL ) }
; static const char_T * rt_LoggedStateLabels [ ] = { "CSTATE" , "CSTATE" ,
"CSTATE" , "CSTATE" , "CSTATE" , "CSTATE" , "CSTATE" , "CSTATE" , "CSTATE" ,
"CSTATE" , "CSTATE" , "CSTATE" , "CSTATE" , "CSTATE" , "CSTATE" , "CSTATE" ,
"states" , "states" , "DSTATE" , "DSTATE" , "DSTATE" , "DSTATE" , "DSTATE" ,
"DSTATE" , "DSTATE" , "DSTATE" , "Discrete" , "Discrete" , "Discrete" ,
"Discrete" , "Discrete" , "Discrete" , "Discrete" , "Discrete" , "Discrete" ,
"Discrete" , "Discrete" , "Discrete" } ; static const char_T *
rt_LoggedStateBlockNames [ ] = {
"openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/joint1" ,
"openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/joint1" ,
"openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/joint2" ,
"openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/joint2" ,
"openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/joint3" ,
"openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/joint3" ,
"openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/joint4" ,
"openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/joint4" ,
"openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/grip_joint" ,
"openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/grip_joint" ,
"openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/grip_joint_sub" ,
"openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/grip_joint_sub" ,
 "openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/Translational Hard Stop1"
,
 "openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/Translational Hard Stop1"
,
 "openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/Translational Hard Stop"
,
 "openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/Translational Hard Stop"
,
 "openManipulatorTrajectoryPlanning/Joint Controller (Configuration Space)/Output Filter"
,
 "openManipulatorTrajectoryPlanning/Joint Controller (Configuration Space)/Input Filter"
,
 "openManipulatorTrajectoryPlanning/Joint Controller (Configuration Space)/PID Compensator \n(Feedback Control)/Discrete PID Controller/Filter/Disc. Forward Euler Filter/Filter"
,
 "openManipulatorTrajectoryPlanning/Joint Controller (Configuration Space)/PID Compensator \n(Feedback Control)/Discrete PID Controller1/Filter/Disc. Forward Euler Filter/Filter"
,
 "openManipulatorTrajectoryPlanning/Joint Controller (Configuration Space)/PID Compensator \n(Feedback Control)/Discrete PID Controller2/Filter/Disc. Forward Euler Filter/Filter"
,
 "openManipulatorTrajectoryPlanning/Joint Controller (Configuration Space)/PID Compensator \n(Feedback Control)/Discrete PID Controller3/Filter/Disc. Forward Euler Filter/Filter"
,
 "openManipulatorTrajectoryPlanning/Joint Controller (Configuration Space)/PID Compensator \n(Feedback Control)/Discrete PID Controller4/Integrator/Discrete/Integrator"
,
 "openManipulatorTrajectoryPlanning/Joint Controller (Configuration Space)/PID Compensator \n(Feedback Control)/Discrete PID Controller5/Integrator/Discrete/Integrator"
,
 "openManipulatorTrajectoryPlanning/Joint Controller (Configuration Space)/Discrete Derivative/UD"
,
 "openManipulatorTrajectoryPlanning/Joint Controller (Configuration Space)/Discrete Derivative1/UD"
,
"openManipulatorTrajectoryPlanning/Solver\nConfiguration/EVAL_KEY/INPUT_5_1_1"
,
"openManipulatorTrajectoryPlanning/Solver\nConfiguration/EVAL_KEY/INPUT_6_1_1"
,
"openManipulatorTrajectoryPlanning/Solver\nConfiguration/EVAL_KEY/INPUT_1_1_1"
,
"openManipulatorTrajectoryPlanning/Solver\nConfiguration/EVAL_KEY/INPUT_2_1_1"
,
"openManipulatorTrajectoryPlanning/Solver\nConfiguration/EVAL_KEY/INPUT_3_1_1"
,
"openManipulatorTrajectoryPlanning/Solver\nConfiguration/EVAL_KEY/INPUT_4_1_1"
,
 "openManipulatorTrajectoryPlanning/Solver\nConfiguration/EVAL_KEY/INTERNAL_3_1_1"
,
 "openManipulatorTrajectoryPlanning/Solver\nConfiguration/EVAL_KEY/INTERNAL_2_1_1"
,
 "openManipulatorTrajectoryPlanning/Solver\nConfiguration/EVAL_KEY/INTERNAL_1_1_1"
,
 "openManipulatorTrajectoryPlanning/Solver\nConfiguration/EVAL_KEY/INTERNAL_6_1_1"
,
 "openManipulatorTrajectoryPlanning/Solver\nConfiguration/EVAL_KEY/INTERNAL_5_1_1"
,
 "openManipulatorTrajectoryPlanning/Solver\nConfiguration/EVAL_KEY/INTERNAL_4_1_1"
} ; static const char_T * rt_LoggedStateNames [ ] = {
"openManipulatorTrajectoryPlanning.Robot_Torque_Actuated.joint1.Rz.q" ,
"openManipulatorTrajectoryPlanning.Robot_Torque_Actuated.joint1.Rz.w" ,
"openManipulatorTrajectoryPlanning.Robot_Torque_Actuated.joint2.Rz.q" ,
"openManipulatorTrajectoryPlanning.Robot_Torque_Actuated.joint2.Rz.w" ,
"openManipulatorTrajectoryPlanning.Robot_Torque_Actuated.joint3.Rz.q" ,
"openManipulatorTrajectoryPlanning.Robot_Torque_Actuated.joint3.Rz.w" ,
"openManipulatorTrajectoryPlanning.Robot_Torque_Actuated.joint4.Rz.q" ,
"openManipulatorTrajectoryPlanning.Robot_Torque_Actuated.joint4.Rz.w" ,
"openManipulatorTrajectoryPlanning.Robot_Torque_Actuated.grip_joint.Pz.p" ,
"openManipulatorTrajectoryPlanning.Robot_Torque_Actuated.grip_joint.Pz.v" ,
"openManipulatorTrajectoryPlanning.Robot_Torque_Actuated.grip_joint_sub.Pz.p"
,
"openManipulatorTrajectoryPlanning.Robot_Torque_Actuated.grip_joint_sub.Pz.v"
,
 "openManipulatorTrajectoryPlanning.Robot_Torque_Actuated.Translational_Hard_Stop1.x"
,
 "openManipulatorTrajectoryPlanning.Robot_Torque_Actuated.Translational_Hard_Stop1.f"
,
 "openManipulatorTrajectoryPlanning.Robot_Torque_Actuated.Translational_Hard_Stop.x"
,
 "openManipulatorTrajectoryPlanning.Robot_Torque_Actuated.Translational_Hard_Stop.f"
, "states" , "states" , "DSTATE" , "DSTATE" , "DSTATE" , "DSTATE" , "DSTATE"
, "DSTATE" , "DSTATE" , "DSTATE" , "Discrete" , "Discrete" , "Discrete" ,
"Discrete" , "Discrete" , "Discrete" , "Discrete" , "Discrete" , "Discrete" ,
"Discrete" , "Discrete" , "Discrete" } ; static boolean_T
rt_LoggedStateCrossMdlRef [ ] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0
, 0 , 0 , 0 , 0 , 0 , 0 , 0 } ; static RTWLogDataTypeConvert
rt_RTWLogDataTypeConvert [ ] = { { 0 , SS_DOUBLE , SS_DOUBLE , 0 , 0 , 0 ,
1.0 , 0 , 0.0 } , { 0 , SS_DOUBLE , SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 , 0.0 } ,
{ 0 , SS_DOUBLE , SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 , 0.0 } , { 0 , SS_DOUBLE ,
SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 , 0.0 } , { 0 , SS_DOUBLE , SS_DOUBLE , 0 , 0
, 0 , 1.0 , 0 , 0.0 } , { 0 , SS_DOUBLE , SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 ,
0.0 } , { 0 , SS_DOUBLE , SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 , 0.0 } , { 0 ,
SS_DOUBLE , SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 , 0.0 } , { 0 , SS_DOUBLE ,
SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 , 0.0 } , { 0 , SS_DOUBLE , SS_DOUBLE , 0 , 0
, 0 , 1.0 , 0 , 0.0 } , { 0 , SS_DOUBLE , SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 ,
0.0 } , { 0 , SS_DOUBLE , SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 , 0.0 } , { 0 ,
SS_DOUBLE , SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 , 0.0 } , { 0 , SS_DOUBLE ,
SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 , 0.0 } , { 0 , SS_DOUBLE , SS_DOUBLE , 0 , 0
, 0 , 1.0 , 0 , 0.0 } , { 0 , SS_DOUBLE , SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 ,
0.0 } , { 0 , SS_DOUBLE , SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 , 0.0 } , { 0 ,
SS_DOUBLE , SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 , 0.0 } , { 0 , SS_DOUBLE ,
SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 , 0.0 } , { 0 , SS_DOUBLE , SS_DOUBLE , 0 , 0
, 0 , 1.0 , 0 , 0.0 } , { 0 , SS_DOUBLE , SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 ,
0.0 } , { 0 , SS_DOUBLE , SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 , 0.0 } , { 0 ,
SS_DOUBLE , SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 , 0.0 } , { 0 , SS_DOUBLE ,
SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 , 0.0 } , { 0 , SS_DOUBLE , SS_DOUBLE , 0 , 0
, 0 , 1.0 , 0 , 0.0 } , { 0 , SS_DOUBLE , SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 ,
0.0 } , { 0 , SS_DOUBLE , SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 , 0.0 } , { 0 ,
SS_DOUBLE , SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 , 0.0 } , { 0 , SS_DOUBLE ,
SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 , 0.0 } , { 0 , SS_DOUBLE , SS_DOUBLE , 0 , 0
, 0 , 1.0 , 0 , 0.0 } , { 0 , SS_DOUBLE , SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 ,
0.0 } , { 0 , SS_DOUBLE , SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 , 0.0 } , { 0 ,
SS_DOUBLE , SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 , 0.0 } , { 0 , SS_DOUBLE ,
SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 , 0.0 } , { 0 , SS_DOUBLE , SS_DOUBLE , 0 , 0
, 0 , 1.0 , 0 , 0.0 } , { 0 , SS_DOUBLE , SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 ,
0.0 } , { 0 , SS_DOUBLE , SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 , 0.0 } , { 0 ,
SS_DOUBLE , SS_DOUBLE , 0 , 0 , 0 , 1.0 , 0 , 0.0 } } ; static int_T
rt_LoggedStateIdxList [ ] = { 0 , 1 , 2 , 3 , 4 , 5 , 6 , 7 , 8 , 9 , 10 , 11
, 12 , 13 , 14 , 15 , 16 , 17 , 18 , 19 , 20 , 21 , 22 , 23 , 24 } ; static
RTWLogSignalInfo rt_LoggedStateSignalInfo = { 38 , rt_LoggedStateWidths ,
rt_LoggedStateNumDimensions , rt_LoggedStateDimensions ,
rt_LoggedStateIsVarDims , ( NULL ) , ( NULL ) , rt_LoggedStateDataTypeIds ,
rt_LoggedStateComplexSignals , ( NULL ) , rt_LoggingStatePreprocessingFcnPtrs
, { rt_LoggedStateLabels } , ( NULL ) , ( NULL ) , ( NULL ) , {
rt_LoggedStateBlockNames } , { rt_LoggedStateNames } ,
rt_LoggedStateCrossMdlRef , rt_RTWLogDataTypeConvert , rt_LoggedStateIdxList
} ; static void * rt_LoggedStateSignalPtrs [ 38 ] ; rtliSetLogXSignalPtrs (
ssGetRTWLogInfo ( rtS ) , ( LogSignalPtrsType ) rt_LoggedStateSignalPtrs ) ;
rtliSetLogXSignalInfo ( ssGetRTWLogInfo ( rtS ) , & rt_LoggedStateSignalInfo
) ; rt_LoggedStateSignalPtrs [ 0 ] = ( void * ) & rtX . ge5rtzzo4y [ 0 ] ;
rt_LoggedStateSignalPtrs [ 1 ] = ( void * ) & rtX . ge5rtzzo4y [ 1 ] ;
rt_LoggedStateSignalPtrs [ 2 ] = ( void * ) & rtX . ge5rtzzo4y [ 2 ] ;
rt_LoggedStateSignalPtrs [ 3 ] = ( void * ) & rtX . ge5rtzzo4y [ 3 ] ;
rt_LoggedStateSignalPtrs [ 4 ] = ( void * ) & rtX . ge5rtzzo4y [ 4 ] ;
rt_LoggedStateSignalPtrs [ 5 ] = ( void * ) & rtX . ge5rtzzo4y [ 5 ] ;
rt_LoggedStateSignalPtrs [ 6 ] = ( void * ) & rtX . ge5rtzzo4y [ 6 ] ;
rt_LoggedStateSignalPtrs [ 7 ] = ( void * ) & rtX . ge5rtzzo4y [ 7 ] ;
rt_LoggedStateSignalPtrs [ 8 ] = ( void * ) & rtX . ge5rtzzo4y [ 8 ] ;
rt_LoggedStateSignalPtrs [ 9 ] = ( void * ) & rtX . ge5rtzzo4y [ 9 ] ;
rt_LoggedStateSignalPtrs [ 10 ] = ( void * ) & rtX . ge5rtzzo4y [ 10 ] ;
rt_LoggedStateSignalPtrs [ 11 ] = ( void * ) & rtX . ge5rtzzo4y [ 11 ] ;
rt_LoggedStateSignalPtrs [ 12 ] = ( void * ) & rtX . lnruxliw5a [ 0 ] ;
rt_LoggedStateSignalPtrs [ 13 ] = ( void * ) & rtX . lnruxliw5a [ 1 ] ;
rt_LoggedStateSignalPtrs [ 14 ] = ( void * ) & rtX . ipa1byflst [ 0 ] ;
rt_LoggedStateSignalPtrs [ 15 ] = ( void * ) & rtX . ipa1byflst [ 1 ] ;
rt_LoggedStateSignalPtrs [ 16 ] = ( void * ) rtDW . jiqh55j12p ;
rt_LoggedStateSignalPtrs [ 17 ] = ( void * ) rtDW . ahio0mgx5h ;
rt_LoggedStateSignalPtrs [ 18 ] = ( void * ) & rtDW . jmlpsyjpqa ;
rt_LoggedStateSignalPtrs [ 19 ] = ( void * ) & rtDW . osgjdwjesl ;
rt_LoggedStateSignalPtrs [ 20 ] = ( void * ) & rtDW . fkcgzk5oma ;
rt_LoggedStateSignalPtrs [ 21 ] = ( void * ) & rtDW . c45aidngho ;
rt_LoggedStateSignalPtrs [ 22 ] = ( void * ) & rtDW . kaentdmtdn ;
rt_LoggedStateSignalPtrs [ 23 ] = ( void * ) & rtDW . d5wogx3kqa ;
rt_LoggedStateSignalPtrs [ 24 ] = ( void * ) rtDW . ggzlanf4wa ;
rt_LoggedStateSignalPtrs [ 25 ] = ( void * ) rtDW . p4ux5yfv5l ;
rt_LoggedStateSignalPtrs [ 26 ] = ( void * ) rtDW . jhwzr3b1pg ;
rt_LoggedStateSignalPtrs [ 27 ] = ( void * ) rtDW . eukf1npmbe ;
rt_LoggedStateSignalPtrs [ 28 ] = ( void * ) rtDW . eutf4o2lqe ;
rt_LoggedStateSignalPtrs [ 29 ] = ( void * ) rtDW . dwwiwqgqz0 ;
rt_LoggedStateSignalPtrs [ 30 ] = ( void * ) rtDW . pmpxho4tev ;
rt_LoggedStateSignalPtrs [ 31 ] = ( void * ) rtDW . eqedaoza0o ;
rt_LoggedStateSignalPtrs [ 32 ] = ( void * ) rtDW . bgoyflaz0e ;
rt_LoggedStateSignalPtrs [ 33 ] = ( void * ) rtDW . liwevcrd2d ;
rt_LoggedStateSignalPtrs [ 34 ] = ( void * ) rtDW . a4s0ocg4tv ;
rt_LoggedStateSignalPtrs [ 35 ] = ( void * ) rtDW . kqczznesoc ;
rt_LoggedStateSignalPtrs [ 36 ] = ( void * ) rtDW . ac1ikcnlza ;
rt_LoggedStateSignalPtrs [ 37 ] = ( void * ) rtDW . mtraeigdsz ; }
rtliSetLogT ( ssGetRTWLogInfo ( rtS ) , "tout" ) ; rtliSetLogX (
ssGetRTWLogInfo ( rtS ) , "" ) ; rtliSetLogXFinal ( ssGetRTWLogInfo ( rtS ) ,
"xFinal" ) ; rtliSetLogVarNameModifier ( ssGetRTWLogInfo ( rtS ) , "none" ) ;
rtliSetLogFormat ( ssGetRTWLogInfo ( rtS ) , 4 ) ; rtliSetLogMaxRows (
ssGetRTWLogInfo ( rtS ) , 0 ) ; rtliSetLogDecimation ( ssGetRTWLogInfo ( rtS
) , 1 ) ; rtliSetLogY ( ssGetRTWLogInfo ( rtS ) , "" ) ;
rtliSetLogYSignalInfo ( ssGetRTWLogInfo ( rtS ) , ( NULL ) ) ;
rtliSetLogYSignalPtrs ( ssGetRTWLogInfo ( rtS ) , ( NULL ) ) ; } { static
struct _ssStatesInfo2 statesInfo2 ; ssSetStatesInfo2 ( rtS , & statesInfo2 )
; } { static ssPeriodicStatesInfo periodicStatesInfo ;
ssSetPeriodicStatesInfo ( rtS , & periodicStatesInfo ) ; } { static
ssJacobianPerturbationBounds jacobianPerturbationBounds ;
ssSetJacobianPerturbationBounds ( rtS , & jacobianPerturbationBounds ) ; } {
static ssSolverInfo slvrInfo ; static struct _ssSFcnModelMethods3 mdlMethods3
; static struct _ssSFcnModelMethods2 mdlMethods2 ; static boolean_T
contStatesDisabled [ 16 ] ; static real_T absTol [ 16 ] = { 0.001 , 0.001 ,
0.001 , 0.001 , 0.001 , 0.001 , 0.001 , 0.001 , 0.001 , 0.001 , 0.001 , 0.001
, 0.001 , 0.001 , 0.001 , 0.001 } ; static uint8_T absTolControl [ 16 ] = {
1U , 1U , 1U , 1U , 1U , 1U , 1U , 1U , 1U , 1U , 1U , 1U , 1U , 1U , 1U , 1U
} ; static real_T contStateJacPerturbBoundMinVec [ 16 ] ; static real_T
contStateJacPerturbBoundMaxVec [ 16 ] ; static ssNonContDerivSigInfo
nonContDerivSigInfo [ 1 ] = { { 6 * sizeof ( real_T ) , ( char * ) ( & rtB .
nd1rapbf2u [ 0 ] ) , ( NULL ) } } ; { int i ; for ( i = 0 ; i < 16 ; ++ i ) {
contStateJacPerturbBoundMinVec [ i ] = 0 ; contStateJacPerturbBoundMaxVec [ i
] = rtGetInf ( ) ; } } ssSetSolverRelTol ( rtS , 0.001 ) ; ssSetStepSize (
rtS , 0.0 ) ; ssSetMinStepSize ( rtS , 0.0 ) ; ssSetMaxNumMinSteps ( rtS , -
1 ) ; ssSetMinStepViolatedError ( rtS , 0 ) ; ssSetMaxStepSize ( rtS , 0.005
) ; ssSetSolverMaxOrder ( rtS , - 1 ) ; ssSetSolverRefineFactor ( rtS , 1 ) ;
ssSetOutputTimes ( rtS , ( NULL ) ) ; ssSetNumOutputTimes ( rtS , 0 ) ;
ssSetOutputTimesOnly ( rtS , 0 ) ; ssSetOutputTimesIndex ( rtS , 0 ) ;
ssSetZCCacheNeedsReset ( rtS , 0 ) ; ssSetDerivCacheNeedsReset ( rtS , 0 ) ;
ssSetNumNonContDerivSigInfos ( rtS , 1 ) ; ssSetNonContDerivSigInfos ( rtS ,
nonContDerivSigInfo ) ; ssSetSolverInfo ( rtS , & slvrInfo ) ;
ssSetSolverName ( rtS , "daessc" ) ; ssSetVariableStepSolver ( rtS , 1 ) ;
ssSetSolverConsistencyChecking ( rtS , 0 ) ; ssSetSolverAdaptiveZcDetection (
rtS , 0 ) ; ssSetSolverRobustResetMethod ( rtS , 0 ) ;
_ssSetSolverUpdateJacobianAtReset ( rtS , true ) ; ssSetAbsTolVector ( rtS ,
absTol ) ; ssSetAbsTolControlVector ( rtS , absTolControl ) ;
ssSetSolverAbsTol_Obsolete ( rtS , absTol ) ;
ssSetSolverAbsTolControl_Obsolete ( rtS , absTolControl ) ;
ssSetJacobianPerturbationBoundsMinVec ( rtS , contStateJacPerturbBoundMinVec
) ; ssSetJacobianPerturbationBoundsMaxVec ( rtS ,
contStateJacPerturbBoundMaxVec ) ; ssSetSolverStateProjection ( rtS , 1 ) ; (
void ) memset ( ( void * ) & mdlMethods2 , 0 , sizeof ( mdlMethods2 ) ) ;
ssSetModelMethods2 ( rtS , & mdlMethods2 ) ; ( void ) memset ( ( void * ) &
mdlMethods3 , 0 , sizeof ( mdlMethods3 ) ) ; ssSetModelMethods3 ( rtS , &
mdlMethods3 ) ; ssSetModelProjection ( rtS , MdlProjection ) ;
ssSetMassMatrixType ( rtS , ( ssMatrixType ) 1 ) ; ssSetMassMatrixNzMax ( rtS
, 14 ) ; ssSetModelMassMatrix ( rtS , MdlMassMatrix ) ;
ssSetModelForcingFunction ( rtS , MdlForcingFunction ) ;
ssSetSolverMassMatrixType ( rtS , ( ssMatrixType ) 1 ) ;
ssSetSolverMassMatrixNzMax ( rtS , 14 ) ; ssSetModelOutputs ( rtS ,
MdlOutputs ) ; ssSetModelUpdate ( rtS , MdlUpdate ) ; ssSetModelDerivatives (
rtS , MdlDerivatives ) ; ssSetSolverMaxConsecutiveMinStep ( rtS , 1 ) ;
ssSetSolverShapePreserveControl ( rtS , 2 ) ; ssSetTNextTid ( rtS , INT_MIN )
; ssSetTNext ( rtS , rtMinusInf ) ; ssSetSolverNeedsReset ( rtS ) ;
ssSetNumNonsampledZCs ( rtS , 0 ) ; ssSetContStateDisabled ( rtS ,
contStatesDisabled ) ; ssSetSolverMaxConsecutiveMinStep ( rtS , 1 ) ; { int_T
* ir = rtMassMatrix . ir ; int_T * jc = rtMassMatrix . jc ; real_T * pr =
rtMassMatrix . pr ; ssSetMassMatrixIr ( rtS , ir ) ; ssSetMassMatrixJc ( rtS
, jc ) ; ssSetMassMatrixPr ( rtS , pr ) ; ( void ) memset ( ( void * ) ir , 0
, 14 * sizeof ( int_T ) ) ; ( void ) memset ( ( void * ) jc , 0 , ( 16 + 1 )
* sizeof ( int_T ) ) ; ( void ) memset ( ( void * ) pr , 0 , 14 * sizeof (
real_T ) ) ; } } ssSetChecksumVal ( rtS , 0 , 791975065U ) ; ssSetChecksumVal
( rtS , 1 , 2127965696U ) ; ssSetChecksumVal ( rtS , 2 , 1703960231U ) ;
ssSetChecksumVal ( rtS , 3 , 721157778U ) ; { static const sysRanDType
rtAlwaysEnabled = SUBSYS_RAN_BC_ENABLE ; static RTWExtModeInfo rt_ExtModeInfo
; static const sysRanDType * systemRan [ 7 ] ; gblRTWExtModeInfo = &
rt_ExtModeInfo ; ssSetRTWExtModeInfo ( rtS , & rt_ExtModeInfo ) ;
rteiSetSubSystemActiveVectorAddresses ( & rt_ExtModeInfo , systemRan ) ;
systemRan [ 0 ] = & rtAlwaysEnabled ; systemRan [ 1 ] = & rtAlwaysEnabled ;
systemRan [ 2 ] = & rtAlwaysEnabled ; systemRan [ 3 ] = & rtAlwaysEnabled ;
systemRan [ 4 ] = & rtAlwaysEnabled ; systemRan [ 5 ] = & rtAlwaysEnabled ;
systemRan [ 6 ] = & rtAlwaysEnabled ; rteiSetModelMappingInfoPtr (
ssGetRTWExtModeInfo ( rtS ) , & ssGetModelMappingInfo ( rtS ) ) ;
rteiSetChecksumsPtr ( ssGetRTWExtModeInfo ( rtS ) , ssGetChecksums ( rtS ) )
; rteiSetTPtr ( ssGetRTWExtModeInfo ( rtS ) , ssGetTPtr ( rtS ) ) ; }
slsaDisallowedBlocksForSimTargetOP ( rtS ,
mr_openManipulatorTrajectoryPlanning_GetSimStateDisallowedBlocks ) ;
slsaGetWorkFcnForSimTargetOP ( rtS ,
mr_openManipulatorTrajectoryPlanning_GetDWork ) ;
slsaSetWorkFcnForSimTargetOP ( rtS ,
mr_openManipulatorTrajectoryPlanning_SetDWork ) ;
rt_RapidReadMatFileAndUpdateParams ( rtS ) ; if ( ssGetErrorStatus ( rtS ) )
{ return rtS ; } return rtS ; }
#if defined(_MSC_VER)
#pragma optimize( "", on )
#endif
const int_T gblParameterTuningTid = 3 ; void MdlOutputsParameterSampleTime (
int_T tid ) { MdlOutputsTID3 ( tid ) ; }
